Sub: tweak Yaw behavior for workign at both high and low gains in an useable way

ArduSub/Attitude.cpp

@@ -34,10 +34,11 @@ void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &ro
 // get_pilot_desired_heading - transform pilot's yaw input into a
 // desired yaw rate
 // returns desired yaw rate in centi-degrees per second
-float Sub::get_pilot_desired_yaw_rate(int16_t stick_angle) const
+float Sub::get_pilot_desired_yaw_rate(float stick_angle) const
 {
     // convert pilot input to the desired yaw rate
-    return stick_angle * g.acro_yaw_p;
+    float max = degrees(attitude_control.get_ang_vel_yaw_max_rads()) * 100.f;
+    return constrain_float(stick_angle * g.acro_yaw_p , -max, max);
 }
 
 // check for ekf yaw reset and adjust target heading

ArduSub/Parameters.cpp

@@ -722,6 +722,7 @@ void Sub::load_parameters()
             AP_Arming::ARMING_CHECK_BATTERY);
     AP_Param::set_default_by_name("CIRCLE_RATE", 2.0f);
     AP_Param::set_default_by_name("ATC_ACCEL_Y_MAX", 110000.0f);
+    AP_Param::set_default_by_name("ATC_RATE_Y_MAX", 180.0f);
     AP_Param::set_default_by_name("RC3_TRIM", 1100);
     AP_Param::set_default_by_name("COMPASS_OFFS_MAX", 1000);
     AP_Param::set_default_by_name("INS_GYR_CAL", 0);

ArduSub/Sub.h

@@ -411,7 +411,7 @@ private:
     void update_altitude();
     float get_smoothing_gain();
     void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
-    float get_pilot_desired_yaw_rate(int16_t stick_angle) const;
+    float get_pilot_desired_yaw_rate(float stick_angle) const;
     void check_ekf_yaw_reset();
     float get_roi_yaw();
     float get_look_ahead_yaw();

ArduSub/control_althold.cpp

@@ -72,7 +72,8 @@ void Sub::handle_attitude()
     } else {
         // If we don't have a mavlink attitude target, we use the pilot's input instead
         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());
-        target_yaw = 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() * gain, channel_yaw->get_radio_trim());
+        target_yaw = get_pilot_desired_yaw_rate(yaw_input);
         if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
             last_roll = ahrs.roll_sensor;
             last_pitch = ahrs.pitch_sensor;

ArduSub/control_auto.cpp

@@ -123,7 +123,7 @@ void Sub::auto_wp_run()
     float target_yaw_rate = 0;
     if (!failsafe.pilot_input) {
         // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
         if (!is_zero(target_yaw_rate)) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
         }
@@ -313,7 +313,7 @@ void Sub::auto_loiter_run()
     // accept pilot input of yaw
     float target_yaw_rate = 0;
     if (!failsafe.pilot_input) {
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
     }
 
     // set motors to full range

ArduSub/control_circle.cpp

@@ -48,7 +48,7 @@ void Sub::circle_run()
 
     // process pilot inputs
     // get pilot's desired yaw rate
-    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
     if (!is_zero(target_yaw_rate)) {
         circle_pilot_yaw_override = true;
     }

ArduSub/control_guided.cpp

@@ -296,7 +296,7 @@ void Sub::guided_pos_control_run()
     float target_yaw_rate = 0;
     if (!failsafe.pilot_input) {
         // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
         if (!is_zero(target_yaw_rate)) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
         }
@@ -349,7 +349,7 @@ void Sub::guided_vel_control_run()
     float target_yaw_rate = 0;
     if (!failsafe.pilot_input) {
         // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
         if (!is_zero(target_yaw_rate)) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
         }
@@ -406,7 +406,7 @@ void Sub::guided_posvel_control_run()
 
     if (!failsafe.pilot_input) {
         // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
         if (!is_zero(target_yaw_rate)) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
         }

ArduSub/control_poshold.cpp

@@ -79,7 +79,8 @@ void Sub::poshold_run()
     // Update attitude //
 
     // get pilot's desired yaw rate
-    float target_yaw_rate = 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() * gain, channel_yaw->get_radio_trim());
+    float target_yaw_rate = 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

ArduSub/control_surface.cpp

@@ -43,7 +43,7 @@ void Sub::surface_run()
     get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
 
     // get pilot's desired yaw rate
-    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input()) * 100;
 
     // call attitude controller
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);