From 5efe94a77159cebe357f6f2bb70a3fab73640798 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 15 Dec 2020 23:18:30 +1030 Subject: [PATCH] Copter: use AP_Math control functions --- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_acro.cpp | 8 ++++---- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_sport.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 52b835cd09..7e6dfbb8dd 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -554,7 +554,7 @@ void Mode::land_run_vertical_control(bool pause_descent) max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. - cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index a73e67f0da..32f23bd910 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -159,15 +159,15 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, if (g.acro_trainer == (uint8_t)Trainer::LIMITED) { const float angle_max = copter.aparm.angle_max; if (roll_angle > angle_max){ - rate_ef_level.x += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); + rate_ef_level.x += sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); }else if (roll_angle < -angle_max) { - rate_ef_level.x += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); + rate_ef_level.x += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); } if (pitch_angle > angle_max){ - rate_ef_level.y += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); + rate_ef_level.y += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); }else if (pitch_angle < -angle_max) { - rate_ef_level.y += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); + rate_ef_level.y += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 597eb5016e..e3bb22675d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -963,7 +963,7 @@ void ModeAuto::loiter_to_alt_run() // Compute a vertical velocity demand such that the vehicle // approaches the desired altitude. - float target_climb_rate = AC_AttitudeControl::sqrt_controller( + float target_climb_rate = sqrt_controller( -alt_error_cm, pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 75de568ea1..c998ccc599 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -54,15 +54,15 @@ void ModeSport::run() const float angle_max = copter.aparm.angle_max; if (roll_angle > angle_max){ - target_roll_rate += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); + target_roll_rate += sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); }else if (roll_angle < -angle_max) { - target_roll_rate += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); + target_roll_rate += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); } if (pitch_angle > angle_max){ - target_pitch_rate += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); + target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); }else if (pitch_angle < -angle_max) { - target_pitch_rate += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); + target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); } // get pilot's desired yaw rate