Sub: use gain-ajusted deadzone for pilot_desired_yaw_rate

ArduPilot · Oct 31, 2023 · abbaae0 · abbaae0
1 parent 5087a42
commit abbaae0
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 3 deletions.
diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp
@@ -64,7 +64,8 @@ void ModeAlthold::run()
     sub.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 = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
+    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
 
     // call attitude controller
     if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input

diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp
@@ -78,7 +78,8 @@ void ModePoshold::run()
     // Update attitude //
 
     // get pilot's desired yaw rate
-    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
+    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats

diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp
@@ -32,7 +32,8 @@ void ModeStabilize::run()
     sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
 
     // get pilot's desired yaw rate
-    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
+    float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
 
     // call attitude controller
     // update attitude controller targets