Análisis y aprendizaje del código fuente de Ardusub (6) - modelo antiguo
althold model: modo de retención de profundidad, que debe usarse con el medidor de profundidad. El modo de estabilización ya se ha discutido en la primera publicación de blog de la serie, por lo que algunos de los mismos detalles en esta parte de la publicación de blog no se discutirán.
Tenga en cuenta que la versión que se explica aquí es maestra y que el plan estable para la versión estable se basa en Sub4.0, y habrá tiempo para actualizar más tarde.
Ingrese la ruta ardupilot \ Ardusub \, busque el archivo control_althold.cpp, el archivo de encabezado interno es Sub.h, y la implementación interna tiene 3 funciones: althold_init (), althold_run () y control_depth (), y luego explique a su vez.
althold_init ()
// althold_init - initialise althold controller
bool Sub::althold_init()
{
if(!control_check_barometer()) {
return false;
}
// initialize vertical speeds and leash lengths
// sets the maximum speed up and down returned by position controller
pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control.set_max_accel_z(g.pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
last_pilot_heading = ahrs.yaw_sensor;
return true;
}
La función de control_check_barometer () es verificar si hay un medidor de profundidad, como se muestra a continuación, si el tablero de control de vuelo no está en el estado de simulación, entonces verificará si el medidor de profundidad existe, si no, pasará gcs (). Send_text () Esta función envía un mensaje de advertencia "No hay ningún medidor de profundidad conectado" a la estación terrestre. Si existe, se juzga el estado de salud del sensor. Si no es saludable, se envía el mensaje de advertencia de "error del medidor de profundidad" de la misma manera. De lo contrario, confirme que el medidor de profundidad esté presente y sea correcto.
bool Sub::control_check_barometer()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
if (!ap.depth_sensor_present) {
// can't hold depth without a depth sensor
gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor is not connected.");
return false;
} else if (failsafe.sensor_health) {
gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor error.");
return false;
}
#endif
return true;
}
Luego está pos_control.set_max_speed_z (-get_pilot_speed_dn (), g.pilot_speed_up) este programa, primero mire la parte del parámetro entre paréntesis:
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t Sub::get_pilot_speed_dn()
{
if (g.pilot_speed_dn == 0) {
return abs(g.pilot_speed_up);
}
return abs(g.pilot_speed_dn);
}
get_pilot_speed_dn () se implementa en Ardusub.cpp. La función interna primero determina si g.pilot_speed_dn es igual a 0. Si es así, entonces g.pilot_speed_up se devuelve como un valor absoluto, si no, g.pilot_speed_dn se toma directamente como un valor absoluto y se devuelve. Para los dos valores, puede ver el archivo Parameters.cpp en la misma ruta.
// @Param: PILOT_SPEED_UP
// @DisplayName: Pilot maximum vertical ascending speed
// @Description: The maximum vertical ascending velocity the pilot may request in cm/s
// @Units: cm/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), // PILOT_VELZ_MAX = 500, 单位cm/s
// @Param: PILOT_SPEED_DN
// @DisplayName: Pilot maximum vertical descending speed
// @Description: The maximum vertical descending velocity the pilot may request in cm/s
// @Units: cm/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
GSCALAR(pilot_speed_dn, "PILOT_SPEED_DN", 0),
// @Param: PILOT_ACCEL_Z
// @DisplayName: Pilot vertical acceleration
// @Description: The vertical acceleration used when pilot is controlling the altitude
// @Units: cm/s/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), // PILOT_VELZ_MAX = 100, 单位cm/s/s
Vuelva a set_max_speed_z () en el programa fuente. Esta función recibe -get_pilot_speed_dn () y g.pilot_speed_up, que son -500 y +500 respectivamente. Luego, después de ingresar a esta función, primero asegúrese de que speed_down sea siempre negativo, y luego compare _speed_down_cms (predeterminado 250.0f), _speed_up_cms (predeterminado 100.0f) y speed_down, speed_up son iguales, si no son iguales, reconfirme la relación positiva y negativa entre los dos parámetros, y luego Asigne valores a _speed_down_cms y _speed_up_cms, y establezca la posición correspondiente en _flags en 1. Solo entonces se puede usar calc_leash_length_z () para calcular la longitud de frenado vertical en función de la velocidad máxima proporcionada por update_z_controller cuando cambia la velocidad del eje z o la aceleración, y luego restablecer _flags.recalc_leash_z a falso después de que se completa el cálculo.
/// set_max_speed_z - set the maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
{
// ensure speed_down is always negative
speed_down = -fabsf(speed_down);
// exit immediately if no change in speed up or down
if (is_equal(_speed_down_cms, speed_down) && is_equal(_speed_up_cms, speed_up)) {
return;
}
// sanity check speeds and update
if (is_positive(speed_up) && is_negative(speed_down)) {
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
_flags.recalc_leash_z = true;
calc_leash_length_z();
}
}
_flags es la estructura de la parte protegida de la clase AC_PosControl:
// general purpose flags
struct poscontrol_flags {
uint16_t recalc_leash_z : 1; // 1 if we should recalculate the z axis leash length
uint16_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length
uint16_t reset_desired_vel_to_pos : 1; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_accel_to_lean_xy : 1; // 1 if we should reset the accel to lean angle step
uint16_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
uint16_t freeze_ff_z : 1; // 1 used to freeze velocity to accel feed forward for one iteration
uint16_t use_desvel_ff_z : 1; // 1 to use z-axis desired velocity as feed forward into velocity step
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
} _flags;
La implementación de la función pos_control.set_max_accel_z (g.pilot_accel_z) es similar a la anterior, por lo que no la repetiré aquí.
La siguiente parte del programa es similar al contenido descrito en la primera publicación de blog de la serie. set_alt_target () establece la última información de altura obtenida de la navegación inercial al valor esperado actual.De manera similar, la velocidad de ascenso esperada actual (cm / s) del eje z abajo, y last_pilot_heading obtiene el último rumbo (ángulo de guiñada).
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
last_pilot_heading = ahrs.yaw_sensor;
althold_run ()
// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{
uint32_t tnow = AP_HAL::millis();
// initialize vertical speeds and acceleration
pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control.set_max_accel_z(g.pilot_accel_z);
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
last_pilot_heading = ahrs.yaw_sensor;
return;
}
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get pilot desired lean angles
float target_roll, target_pitch;
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = degrees(target_roll);
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
if (!is_zero(target_yaw_rate)) {
// call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else {
// hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) {
// give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else {
// call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}
control_depth();
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}
Comparando el análisis del código fuente de Ardusub y el aprendizaje (1) -el modelo de estabilización mencionado en el programa principal de Ardusub , podemos ver que el marco general es similar, pero algunos programas se agregan sobre la base de la estabilización.Aquí hablaremos de algunos de ellos.
Tenga en cuenta que es equivalente a agregar los siguientes segmentos de programa a motors.set_desired_spool_state () y get_pilot_desired_lean_angles () de stabilized.
// get pilot desired lean angles
float target_roll, target_pitch;
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = degrees(target_roll);
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
}
Este programa se utiliza para determinar si existe un sistema de ubicación GPS y habilitar el control de actitud y profundidad sin un sistema de ubicación. Y convierta el cuaternión en el paquete mavlink en ángulos de Euler expresados en grados para controlar la actitud del ángulo RPY.
La última parte es similar a la parte del modo de estabilización, es decir, enciende el controlador de actitud y configura el control del motor de los canales de avance y retroceso. Es solo que el control del acelerador se elimina al final y se agrega la función control_depth () A continuación, analicemos el programa en detalle.
control_depth ()
void Sub::control_depth() {
// Hold actual position until zero derivative is detected
static bool engageStopZ = true;
// Get last user velocity direction to check for zero derivative points
static bool lastVelocityZWasNegative = false;
if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) {
// Throttle input above 5%
// output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
// reset z targets to current values
pos_control.relax_alt_hold_controllers();
engageStopZ = true;
lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
} else {
// hold z
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(); // clear velocity and position targets
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
}
// Detects a zero derivative
// When detected, move the altitude set point to the actual position
// This will avoid any problem related to joystick delays
// or smaller input signals
if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
engageStopZ = false;
pos_control.relax_alt_hold_controllers();
}
pos_control.update_z_controller();
}
}
El proceso de control en profundidad es el siguiente:
- Primero, mantenga la posición actual hasta que se detecte la derivada cero, y obtenga la dirección de velocidad final del usuario para verificar el punto de derivada cero. Específicamente, establezca la posición de la bandera EngancharStopZ en verdadero y establezca lastVelocityZWasNegative en falso;
- Luego juzgue si la cantidad de entrada del canal de entrada del acelerador utilizado para controlar las subidas y bajadas alcanza el 5%;
- Si excede el 5%, se considera que el control de profundidad en la dirección z se realiza y la entrada del acelerador del canal se obtiene y se establece mediante la función actitud_control.set_throttle_out (), y luego los parámetros esperados y de destino en la dirección del eje z se restablecen mediante pos_control.relax_alt_hold_controllers () Es el valor actual y luego establece el bit de bandera correspondiente;
- Si no supera el 5%, la entrada actual se considera inválida y se mantiene el estado de profundidad actual:
- Primero, determine si el vehículo submarino ha tocado fondo, si ha tocado fondo, borre y restablezca el valor esperado en el eje z, y restablezca la posición de actitud objetivo a 10 cm desde el fondo;
- Si no llega al fondo, detecte la derivada cero y restablezca los parámetros esperados y de destino en la dirección del eje Z al valor actual cuando se detecte. Esto puede evitar cualquier problema relacionado con el retraso del joystick o pequeñas señales de entrada;
- Finalmente, actualice y ejecute el controlador de posición en la dirección del eje z.
El siguiente es el procedimiento interno de relax_alt_hold_controllers ():
/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl_Sub::relax_alt_hold_controllers()
{
_pos_target.z = _inav.get_altitude();
_vel_desired.z = 0.0f;
_flags.use_desvel_ff_z = false;
_vel_target.z = _inav.get_velocity_z();
_vel_last.z = _inav.get_velocity_z();
_accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f;
_flags.reset_rate_to_accel_z = true;
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
}
Programa de controlador de posición del eje Z:
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check time since last cast
const uint64_t now_us = AP_HAL::micros64();
if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
_flags.reset_rate_to_accel_z = true;
_pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
}
_last_update_z_us = now_us;
// check for ekf altitude reset
check_for_ekf_z_reset();
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call z-axis position controller
run_z_controller();
}
Habrá tiempo para actualizar algo
más tarde. Primera actualización: 2020/10/11