Sub: introduce CONTROL_FRAME parameter, allowing users to switch control frame for yaw

ArduSub/Parameters.cpp

@@ -407,6 +407,14 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Advanced
     GSCALAR(acro_expo,  "ACRO_EXPO",    ACRO_EXPO_DEFAULT),
 
+    // @Param: CONTROL_FRAME
+    // @DisplayName: Control Frame
+    // @Description: Control Frame used in stabilized modes
+    // @Values: 0: Standard, 1: Body frame Yaw/Pitch/Roll
+    // @User: Standard
+
+    GSCALAR(control_frame,  "CONTROL_FRAME",    CONTROL_FRAME_DEFAULT),
+
     // variables not in the g class which contain EEPROM saved variables
 
 #if CAMERA == ENABLED

ArduSub/Parameters.h

@@ -191,6 +191,7 @@ public:
         k_param_throttle_gain,
         k_param_cam_tilt_center, // deprecated
         k_param_frame_configuration,
+        k_param_control_frame,
 
         // Acro Mode parameters
         k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate
@@ -302,6 +303,7 @@ public:
 
     AP_Float                surface_depth;
     AP_Int8                 frame_configuration;
+    AP_Int8                 control_frame;
 
     // Note: keep initializers here in the same order as they are declared
     // above.

ArduSub/config.h

@@ -153,6 +153,11 @@
 #define ACRO_EXPO_DEFAULT          0.3f
 #endif
 
+// should be a subset of MAV_FRAME
+#ifndef CONTROL_FRAME_DEFAULT
+#define CONTROL_FRAME_DEFAULT      0.0f
+#endif
+
 // AUTO Mode
 #ifndef WP_YAW_BEHAVIOR_DEFAULT
 # define WP_YAW_BEHAVIOR_DEFAULT   WP_YAW_BEHAVIOR_CORRECT_XTRACK

ArduSub/control_althold.cpp

@@ -50,7 +50,7 @@ void Sub::handle_attitude()
     pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // get pilot desired lean angles
+    // get pilot desired lean angles/rates
     float target_roll, target_pitch, target_yaw;
 
     // Check if set_attitude_target_no_gps is valid
@@ -69,23 +69,57 @@ void Sub::handle_attitude()
         last_pitch = target_pitch;
         last_pilot_heading = target_yaw;
         attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
-    } else {
+        return;
-        // 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());
+    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());
-        float yaw_input =  channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim());
+    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);
+    target_yaw = get_pilot_desired_yaw_rate(yaw_input);
-        if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
+    // If we don't have a mavlink attitude target, we use the pilot's input instead
-            last_roll = ahrs.roll_sensor;
+    switch (g.control_frame) {
-            last_pitch = ahrs.pitch_sensor;
+        case MAV_FRAME_BODY_FRD:
-            last_pilot_heading = ahrs.yaw_sensor;
+        {
-            last_input_ms = tnow;
+          if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
-            attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+              last_input_ms = tnow;
-        } else if (tnow < last_input_ms + 250) {
+              attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
-            // just brake for a few mooments so we don't bounce
+              Quaternion attitude_target = attitude_control.get_attitude_target_quat();
-            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
+              last_roll = degrees(attitude_target.get_euler_roll()) * 100;
-        } else {
+              last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
-            // Lock attitude
+              last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
-            attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+          } else {
+              attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+          } 
+        }
+        break;
+        default:
+        {
+            // call attitude controller
+            if (!is_zero(target_yaw)) { // call attitude controller with rate yaw determined by pilot input
+                attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, target_yaw);
+                last_pilot_heading = ahrs.yaw_sensor;
+                last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
+
+            } else { // hold current heading
+
+                if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
+                    last_roll = ahrs.roll_sensor;
+                    last_pitch = ahrs.pitch_sensor;
+                    last_pilot_heading = ahrs.yaw_sensor;
+                    last_input_ms = tnow;
+                    attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+
+                // this check is required to prevent bounce back after very fast yaw maneuvers
+                // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
+                } else if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
+                    target_yaw = 0; // Stop rotation on yaw axis
+
+                    // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
+                    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, target_yaw);
+                    last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
+
+                } else { // call attitude controller holding absolute absolute bearing
+                    attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+                }
+            }
         }
     }
 }
@@ -99,6 +133,7 @@ void Sub::althold_run()
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
         attitude_control.set_throttle_out(0.75,true,100.0);
+        attitude_control.relax_attitude_controllers();
         pos_control.init_z_controller();
         // initialise position and desired velocity
         float pos = stopping_distance();

ArduSub/joystick.cpp

@@ -1,5 +1,7 @@
 #include "Sub.h"
 
+#define TRIM_STEP 0.5 // trim step in degrees
+
 // Functions that will handle joystick/gamepad input
 // ----------------------------------------------------------------------------
 
@@ -141,12 +143,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
 {
     // Used for trimming level in vehicle frame
-    Quaternion attitudeTarget;
+    Quaternion attitude_target = attitude_control.get_attitude_target_quat();
-    attitudeTarget.from_euler(
+
-        radians(last_roll * 0.01f),
-        radians(last_pitch * 0.01f),
-        radians(last_pilot_heading * 0.01f)
-        );
     Vector3f localPitch = Vector3f(0, 1, 0);
     Vector3f localRoll = Vector3f(1, 0, 0);
 
@@ -340,28 +338,41 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
         }
         break;
     case JSButton::button_function_t::k_trim_roll_inc:
-        attitudeTarget.rotate(localRoll * radians(1));
+        last_roll += TRIM_STEP * 100;
-        last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
+        if (g.control_frame == MAV_FRAME_BODY_FRD) {
-        last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
+            // TODO: change these to use input_quaternion()
-        last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
+            attitude_target.rotate(localRoll * radians(TRIM_STEP));
+            last_roll = degrees(attitude_target.get_euler_roll()) * 100;
+            last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
+            last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
+        }
         break;
     case JSButton::button_function_t::k_trim_roll_dec:
-        attitudeTarget.rotate(localRoll * radians(-1));
+        last_roll -= TRIM_STEP * 100;
-        last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
+        if (g.control_frame == MAV_FRAME_BODY_FRD) {
-        last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
+            attitude_target.rotate(localRoll * radians(-TRIM_STEP));
-        last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
+            last_roll = degrees(attitude_target.get_euler_roll()) * 100;
+            last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
+            last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
+        }
         break;
     case JSButton::button_function_t::k_trim_pitch_inc:
-        attitudeTarget.rotate(localPitch * radians(1));
+            last_pitch += TRIM_STEP * 100;
-        last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
+        if (g.control_frame == MAV_FRAME_BODY_FRD) {
-        last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
+            attitude_target.rotate(localPitch * radians(TRIM_STEP));
-        last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
+            last_roll = degrees(attitude_target.get_euler_roll()) * 100;
+            last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
+            last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
+        }
         break;
     case JSButton::button_function_t::k_trim_pitch_dec:
-        attitudeTarget.rotate(localPitch * radians(-1));
+        last_pitch -= TRIM_STEP * 100;
-        last_roll = degrees(attitudeTarget.get_euler_roll()) * 100;
+        if (g.control_frame == MAV_FRAME_BODY_FRD) {
-        last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100;
+            attitude_target.rotate(localPitch * radians(-TRIM_STEP));
-        last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100;
+            last_roll = degrees(attitude_target.get_euler_roll()) * 100;
+            last_pitch = degrees(attitude_target.get_euler_pitch()) * 100;
+            last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100;
+        }
         break;
     case JSButton::button_function_t::k_input_hold_set:
         if(!motors.armed()) {