Ardusub source code analysis and learning (6)-althold model
althold model: depth hold mode, which needs to be used with depth gauge. The stabilize mode has already been discussed in the first blog post of the series, so some of the same details in this part of the blog post will not be discussed.
Note that the version explained here is master, and the stable plan for the stable version is based on Sub4.0, and there will be time to update later.
Enter the ardupilot\Ardusub\ path, find the control_althold.cpp file, the internal header file is Sub.h, and the internal implementation has 3 functions: althold_init(), althold_run() and control_depth(), and then explain in turn.
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;
}
The function of control_check_barometer() is to check whether there is a depth gauge, as shown below, if the flight control board is not in the simulation state, then it will check whether the depth gauge exists, if not, it will pass gcs().send_text() This function sends a warning message "No depth gauge is connected" to the ground station. If it exists, the health status of the sensor is judged. If it is unhealthy, the warning message of "depth gauge error" is sent in the same way. Otherwise, confirm that the depth gauge is present and correct.
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;
}
Then there is pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up) this program, first look at the parameter part in brackets:
// 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() is implemented in Ardusub.cpp. The internal function first determines whether g.pilot_speed_dn is equal to 0. If yes, then g.pilot_speed_up is returned as an absolute value, if not, g.pilot_speed_dn is directly taken as an absolute value and returned. For the two values, you can view the Parameters.cpp file under the same path.
// @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
Go back to set_max_speed_z() in the source program. This function receives -get_pilot_speed_dn() and g.pilot_speed_up, which are -500 and +500 respectively. Then after entering this function, first make sure that speed_down is always negative, and then compare _speed_down_cms (default 250.0f), _speed_up_cms (default 100.0f) and speed_down, speed_up are equal, if not equal, reconfirm the positive and negative relationship between the two parameters, and then Assign values to _speed_down_cms and _speed_up_cms, and set the corresponding position in _flags to 1. Only then can calc_leash_length_z() be used to calculate the vertical braking length based on the maximum speed provided by update_z_controller when the z-axis speed or acceleration changes, and then reset _flags.recalc_leash_z to false after the calculation is completed.
/// 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 is the structure of the protected part of the AC_PosControl class:
// 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;
The implementation of the pos_control.set_max_accel_z(g.pilot_accel_z) function is similar to the above, so I won’t repeat it here.
The following part of the program is similar to the content described in the first blog post in the series. set_alt_target() sets the latest height information obtained from the inertial navigation to the current expected value. Similarly, the current expected climb rate (cm/s) of the z-axis below, and last_pilot_heading obtains the last heading (yaw angle).
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());
}
Comparing Ardusub source code analysis and learning (1)-the stabilize model mentioned in the Ardusub main program , we can see that the overall framework is similar, but some programs are added on the basis of stabilization. Here we will talk about some of them.
Note that it is equivalent to adding the following program segments to motors.set_desired_spool_state() and get_pilot_desired_lean_angles() of 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;
}
This program is used to determine whether there is a GPS location system, and enable attitude and depth control without a location system. And convert the quaternion in the mavlink package into Euler angles expressed in degrees to control the attitude of the RPY angle.
The latter part is similar to the stabilize mode part, that is, turn on the attitude controller and set the motor control of the forward and backward channels. It's just that the throttle control is removed at the end and the control_depth() function is added. Next, let's analyze the program in detail.
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();
}
}
The depth control process is as follows:
- First, keep the actual position until the zero derivative is detected, and obtain the final user speed direction to check the zero derivative point. Specifically, set the position of the engageStopZ flag to true and set lastVelocityZWasNegative to false;
- Then judge whether the input amount of the throttle input channel used to control the ups and downs reaches 5%;
- If it exceeds 5%, then it is judged that the depth control in the z direction is performed, and the channel throttle input is obtained and set by the attitude_control.set_throttle_out() function, and then the expected and target parameters in the z-axis direction are reset by pos_control.relax_alt_hold_controllers() Is the current value, and then set the corresponding flag bit;
- If it does not exceed 5%, the current input is judged to be invalid and the current depth state is maintained:
- First judge whether the underwater vehicle has bottomed out. If it has bottomed out, clear and reset the expected value on the z-axis, and reset the target attitude position to 10cm from the bottom;
- If there is no bottoming, detect the zero derivative, and reset the expected and target parameters in the z-axis direction to the current value when it is detected. This can avoid any problems related to joystick delay or small input signals;
- Finally, update and run the position controller in the z-axis direction.
The following is the internal procedure of 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();
}
Z-axis position controller program:
/// 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();
}
There will be time to update something
later. First update: 2020/10/11