/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
void AC_PosControl::set_target_to_stopping_point_z()
{
// check if z leash needs to be recalculated
calc_leash_length_z();
get_stopping_point_z(_pos_target);
}
/// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
void AC_PosControl::set_max_accel_z(float accel_cmss)
{
if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) {
_accel_z_cms = accel_cmss;
_flags.recalc_leash_z = true;
calc_leash_length_z();
}
}
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
pos_to_rate_z();
}
/// 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);
if ((fabsf(_speed_down_cms-speed_down) > 1.0f) || (fabsf(_speed_up_cms-speed_up) > 1.0f)) {
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
_flags.recalc_leash_z = true;
calc_leash_length_z();
}
}
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check time since last cast
uint32_t now = hal.scheduler->millis();
if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
_last_update_z_ms = now;
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
pos_to_rate_z();
}
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check time since last cast
uint32_t now = AP_HAL::millis();
if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
_last_update_z_ms = now;
// 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();
}
///////////////////////////////////////////////////////////NEW_FUNCTIONS////////////////////////////////////
void AC_PosControl::update_z_controller_new()
{
// check time since last cast
uint32_t now = hal.scheduler->millis();
if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
_last_update_z_ms = now;
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
//z_values includes current_z, desired_z, pos_error_z respectively
pos_to_rate_z_new();
}