From f3dc47ce3d73c9055453717ad71067649afffa03 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 6 Sep 2021 19:14:18 +0930 Subject: [PATCH] Copter: Add units to the AC_AttitudeControl Library --- ArduCopter/mode.cpp | 4 ++-- ArduCopter/mode_acro.cpp | 8 ++++---- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_autotune.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_guided.cpp | 2 +- ArduCopter/mode_land.cpp | 2 +- ArduCopter/mode_loiter.cpp | 4 ++-- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_rtl.cpp | 2 +- ArduCopter/mode_sport.cpp | 8 ++++---- ArduCopter/mode_systemid.cpp | 4 ++-- ArduCopter/mode_zigzag.cpp | 4 ++-- ArduPlane/mode_qloiter.cpp | 2 +- ArduSub/control_althold.cpp | 2 +- ArduSub/control_guided.cpp | 2 +- 16 files changed, 26 insertions(+), 26 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 0ca6e7f42a..1e18b0ab8d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -623,7 +623,7 @@ void Mode::land_run_horizontal_control() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -737,7 +737,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos) float target_roll = 0.0f; float target_pitch = 0.0f; // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 5014b12674..d6c010d8d5 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 += 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_cdss(), G_Dt); }else if (roll_angle < -angle_max) { - rate_ef_level.x += 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_cdss(), G_Dt); } if (pitch_angle > angle_max){ - rate_ef_level.y += 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_cdss(), G_Dt); }else if (pitch_angle < -angle_max) { - rate_ef_level.y += 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_cdss(), G_Dt); } } diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index a1aa76f40c..3f7422a736 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -33,7 +33,7 @@ void ModeAltHold::run() // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd()); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index e1ca2680bb..1d054fef96 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -84,7 +84,7 @@ float AutoTune::get_pilot_desired_climb_rate_cms(void) const void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds) { copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max, - copter.attitude_control->get_althold_lean_angle_max()); + copter.attitude_control->get_althold_lean_angle_max_cd()); yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in()); } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index e2714a7df2..474b93f524 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -315,7 +315,7 @@ void ModeFlowHold::run() int16_t roll_in = copter.channel_roll->get_control_in(); int16_t pitch_in = copter.channel_pitch->get_control_in(); float angle_max = copter.aparm.angle_max; - get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max_cd()); if (quality_filtered >= flow_min_quality && AP_HAL::millis() - copter.arm_time_ms > 3000) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 717381af85..60e2c976c9 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -806,7 +806,7 @@ void ModeGuided::angle_control_run() float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max); + float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), copter.aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 3d59afb071..76111be827 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -110,7 +110,7 @@ void ModeLand::nogps_run() update_simple_mode(); // get pilot desired lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd()); } // get pilot's desired yaw rate diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index a2acd4618e..8a267ccc3a 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -15,7 +15,7 @@ bool ModeLoiter::init(bool ignore_checks) update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); @@ -90,7 +90,7 @@ void ModeLoiter::run() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 981544c967..fbdd982893 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -80,7 +80,7 @@ void ModePosHold::run() // convert pilot input to lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd()); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 01fe719db7..e4708d5225 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -319,7 +319,7 @@ void ModeRTL::descent_run() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index aedb88793c..be502da39a 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -50,15 +50,15 @@ void ModeSport::run() const float angle_max = copter.aparm.angle_max; if (roll_angle > angle_max){ - target_roll_rate += 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_cdss(), G_Dt); }else if (roll_angle < -angle_max) { - target_roll_rate += 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_cdss(), G_Dt); } if (pitch_angle > angle_max){ - target_pitch_rate += 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_cdss(), G_Dt); }else if (pitch_angle < -angle_max) { - target_pitch_rate += 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_cdss(), G_Dt); } // get pilot's desired yaw rate diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 98e09bf12c..1c782c3c72 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -185,9 +185,9 @@ void ModeSystemId::run() gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: Landed"); break; } - if (attitude_control->lean_angle()*100 > attitude_control->lean_angle_max()) { + if (attitude_control->lean_angle_deg()*100 > attitude_control->lean_angle_max_cd()) { systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; - gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle(), (double)attitude_control->lean_angle_max()); + gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle_deg(), (double)attitude_control->lean_angle_max_cd()); break; } if (waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) { diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 12f785ff07..e750e4a70f 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -73,7 +73,7 @@ bool ModeZigZag::init(bool ignore_checks) // convert pilot input to lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); @@ -300,7 +300,7 @@ void ModeZigZag::manual_control() update_simple_mode(); // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 0a1e65ce9b..25cfcf55d5 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -63,7 +63,7 @@ void ModeQLoiter::run() // process pilot's roll and pitch input float target_roll_cd, target_pitch_cd; - quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd); // run loiter controller diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index c72a2401e8..43770f38a0 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -67,7 +67,7 @@ void Sub::althold_run() 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_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max_cd()); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index f5128f7af8..f5459921d8 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -470,7 +470,7 @@ void Sub::guided_angle_control_run() float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); + float angle_max = MIN(attitude_control.get_althold_lean_angle_max_cd(), aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio;