From a72182ae972fdd7c1939e6bc0fe5953717b59a24 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 19 Sep 2024 23:33:02 +0100 Subject: [PATCH] Copter: don't pass `channel_yaw->norm_input_dz()` into `get_pilot_desired_yaw_rate` --- ArduCopter/autoyaw.cpp | 2 +- ArduCopter/mode.cpp | 5 ++++- ArduCopter/mode.h | 2 +- ArduCopter/mode_acro_heli.cpp | 2 +- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_autorotate.cpp | 2 +- ArduCopter/mode_autotune.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_loiter.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_sport.cpp | 2 +- ArduCopter/mode_stabilize.cpp | 2 +- ArduCopter/mode_stabilize_heli.cpp | 2 +- ArduCopter/mode_systemid.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 4 ++-- 15 files changed, 19 insertions(+), 16 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 63e1b277aa..e63c08fe7b 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -314,7 +314,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading() _pilot_yaw_rate_cds = 0.0; if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) { // get pilot's desired yaw rate - _pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz()); + _pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate(); if (!is_zero(_pilot_yaw_rate_cds)) { auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5788ac42a6..74e3cbb3c0 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -1035,13 +1035,16 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms) // transform pilot's yaw input into a desired yaw rate // returns desired yaw rate in centi-degrees per second -float Mode::get_pilot_desired_yaw_rate(float yaw_in) +float Mode::get_pilot_desired_yaw_rate() const { // throttle failsafe check if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) { return 0.0f; } + // Get yaw input + const float yaw_in = channel_yaw->norm_input_dz(); + // convert pilot input to the desired yaw rate return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo()); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 77d72c7073..9f451ad0ad 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -163,7 +163,7 @@ public: // pilot input processing void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const; Vector2f get_pilot_desired_velocity(float vel_max) const; - float get_pilot_desired_yaw_rate(float yaw_in); + float get_pilot_desired_yaw_rate() const; float get_pilot_desired_throttle() const; // returns climb target_rate reduced to avoid obstacles and diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 8c06786513..069de0c6c6 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -111,7 +111,7 @@ void ModeAcro_Heli::run() // if there is no external gyro then run the usual // ACRO_YAW_P gain on the input control, including // deadzone - yaw_in = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + yaw_in = get_pilot_desired_yaw_rate(); } // run attitude controller diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 09e5ad0a88..357d701f95 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -36,7 +36,7 @@ void ModeAltHold::run() 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->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 624e879158..cc97d75f30 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -282,7 +282,7 @@ void ModeAutorotate::run() get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // Get pilot's desired yaw rate - float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + float pilot_yaw_rate = get_pilot_desired_yaw_rate(); // Pitch target is calculated in autorotation phase switch above attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 2c0ca76e7e..a997e1ea1c 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -75,7 +75,7 @@ void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitc { 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_cd()); - yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz()); + yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(); } /* diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 8384f06f71..467ef545cc 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -248,7 +248,7 @@ void ModeFlowHold::run() target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); // Flow Hold State Machine Determination AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 24863a644f..752f4f6115 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -102,7 +102,7 @@ void ModeLoiter::run() loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 2774d42751..bf514c61cd 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -83,7 +83,7 @@ void ModePosHold::run() 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->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot desired climb rate (for alt-hold mode and take-off) float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 83366a9e86..249fa06ee5 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -62,7 +62,7 @@ void ModeSport::run() } // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 5dbf2d4bdc..301bf991f4 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -16,7 +16,7 @@ void ModeStabilize::run() get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); if (!motors->armed()) { // Motors should be Stopped diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index db9471616e..4260e4f9aa 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -31,7 +31,7 @@ void ModeStabilize_Heli::run() get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + float target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot's desired throttle pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index a37a43995e..db1e0ddc26 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -176,7 +176,7 @@ void ModeSystemId::run() get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_yaw_rate = get_pilot_desired_yaw_rate(); if (!motors->armed()) { // Motors should be Stopped diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index d3ea90b545..57f274b2cc 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -264,7 +264,7 @@ void ModeZigZag::auto_control() float target_yaw_rate = 0; if (!copter.failsafe.radio) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_yaw_rate = get_pilot_desired_yaw_rate(); } // set motors to full range @@ -305,7 +305,7 @@ void ModeZigZag::manual_control() // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()); + target_yaw_rate = get_pilot_desired_yaw_rate(); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());