Copter: simple mode fix

Replaces update_simple_mode with apply_simple_mode function that takes roll and pitch input arguments.  The rotation is now done within the various get-pilot-desired-xxx methods removing the need for each flight mode to call update_simple_mode and also removing the slightly ugly call to overwrite RC_Channels' control inputs from the flight code (e.g. no more calls to set_control_in)

Author: rmackay9

ArduCopter/Copter.cpp

@@ -850,39 +850,6 @@ void Copter::init_simple_bearing()
 #endif
 }
 
-// update_simple_mode - rotates pilot input if we are in simple mode
-void Copter::update_simple_mode(void)
-{
-    float rollx, pitchx;
-
-    // exit immediately if no new radio frame or not in simple mode
-    if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
-        return;
-    }
-
-    // mark radio frame as consumed
-    ap.new_radio_frame = false;
-
-    // avoid processing bind-time RC values:
-    if (!rc().has_valid_input()) {
-        return;
-    }
-
-    if (simple_mode == SimpleMode::SIMPLE) {
-        // rotate roll, pitch input by -initial simple heading (i.e. north facing)
-        rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
-        pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
-    }else{
-        // rotate roll, pitch input by -super simple heading (reverse of heading to home)
-        rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
-        pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
-    }
-
-    // rotate roll, pitch input from north facing to vehicle's perspective
-    channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
-    channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
-}
-
 // update_super_simple_bearing - adjusts simple bearing based on location
 // should be called after home_bearing_rad has been updated
 void Copter::update_super_simple_bearing(bool force_update)

ArduCopter/Copter.h

@@ -722,7 +722,6 @@ class Copter : public AP_Vehicle {
     void three_hz_loop();
     void one_hz_loop();
     void init_simple_bearing();
-    void update_simple_mode(void);
     void update_super_simple_bearing(bool force_update);
     void read_AHRS(void);
     void update_altitude();

ArduCopter/mode.cpp

@@ -563,8 +563,15 @@ void Mode::get_pilot_desired_lean_angles_rad(float &roll_out_rad, float &pitch_o
         return;
     }
 
-    //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
-    rc_input_to_roll_pitch_rad(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), angle_max_rad,  angle_limit_rad, roll_out_rad, pitch_out_rad);
+    // fetch roll and pitch inputs
+    float roll_in_norm = channel_roll->norm_input_dz();
+    float pitch_in_norm = channel_pitch->norm_input_dz();
+
+    // apply SIMPLE mode transform to pilot inputs
+    apply_simple_mode(roll_in_norm, pitch_in_norm);
+
+    // transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
+    rc_input_to_roll_pitch_rad(roll_in_norm, pitch_in_norm, angle_max_rad, angle_limit_rad, roll_out_rad, pitch_out_rad);
 }
 
 // transform pilot's roll or pitch input into a desired velocity
@@ -579,6 +586,9 @@ Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
     float roll_out_norm = channel_roll->norm_input_dz();
     float pitch_out_norm = channel_pitch->norm_input_dz();
 
+    // apply SIMPLE mode transform to pilot inputs
+    apply_simple_mode(roll_out_norm, pitch_out_norm);
+
     // convert roll and pitch inputs into velocity in NE frame
     vel_ne_ms = Vector2f(-pitch_out_norm, roll_out_norm);
     if (vel_ne_ms.is_zero()) {
@@ -789,9 +799,6 @@ void Mode::land_run_horizontal_control()
         }
 
         if (g.land_repositioning) {
-            // apply SIMPLE mode transform to pilot inputs
-            update_simple_mode();
-
             // convert pilot input to reposition velocity
             // use half maximum acceleration as the maximum velocity to ensure aircraft will
             // stop from full reposition speed in less than 1 second.
@@ -1094,9 +1101,30 @@ float Mode::get_non_takeoff_throttle() const
     return copter.get_non_takeoff_throttle();
 }
 
-// Updates simple/super-simple heading reference based on current yaw and mode.
-void Mode::update_simple_mode(void) {
-    copter.update_simple_mode();
+// Rotates roll/pitch pilot input if simple or super simple mode is active.
+// roll and pitch may be in any units/scale; the rotation is scale-independent
+void Mode::apply_simple_mode(float &roll, float &pitch) const
+{
+    // exit immediately if not in simple mode
+    if (copter.simple_mode == Copter::SimpleMode::NONE) {
+        return;
+    }
+
+    float roll_out, pitch_out;
+
+    if (copter.simple_mode == Copter::SimpleMode::SIMPLE) {
+        // rotate roll, pitch input by -initial simple heading (i.e. north facing)
+        roll_out = roll*copter.simple_cos_yaw - pitch*copter.simple_sin_yaw;
+        pitch_out = roll*copter.simple_sin_yaw + pitch*copter.simple_cos_yaw;
+    } else {
+        // rotate roll, pitch input by -super simple heading (reverse of heading to home)
+        roll_out = roll*copter.super_simple_cos_yaw - pitch*copter.super_simple_sin_yaw;
+        pitch_out = roll*copter.super_simple_sin_yaw + pitch*copter.super_simple_cos_yaw;
+    }
+
+    // rotate roll, pitch input from north facing to vehicle's perspective
+    roll = roll_out*copter.ahrs.cos_yaw() + pitch_out*copter.ahrs.sin_yaw();
+    pitch = -roll_out*copter.ahrs.sin_yaw() + pitch_out*copter.ahrs.cos_yaw();
 }
 
 // Requests a mode change with the specified reason; returns true if accepted.

ArduCopter/mode.h

@@ -405,8 +405,8 @@ class Mode {
     // Returns the throttle level to maintain altitude (excluding takeoff boost).
     float get_non_takeoff_throttle() const;
 
-    // Updates simple/super-simple heading reference based on current yaw and mode.
-    void update_simple_mode();
+    // Rotates roll/pitch pilot input if simple or super simple mode is active.
+    void apply_simple_mode(float &roll, float &pitch) const;
 
     // Requests a mode change with the specified reason; returns true if accepted.
     bool set_mode(Mode::Number mode, ModeReason reason);

ArduCopter/mode_acro.cpp

@@ -108,6 +108,9 @@ void ModeAcro::get_pilot_desired_rates_rads(float &roll_out_rads, float &pitch_o
     float pitch_in_norm = channel_pitch->norm_input_dz();
     const float yaw_in_norm = channel_yaw->norm_input_dz();
 
+    // apply SIMPLE mode transform to pilot inputs
+    apply_simple_mode(roll_in_norm, pitch_in_norm);
+
     // apply circular limit to pitch and roll inputs
     float norm_in_length = norm(pitch_in_norm, roll_in_norm);
 

ArduCopter/mode_althold.cpp

@@ -29,9 +29,6 @@ void ModeAltHold::run()
     // set vertical speed and acceleration limits
     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
 
-    // apply SIMPLE mode transform to pilot inputs
-    update_simple_mode();
-
     // get pilot desired lean angles
     float target_roll_rad, target_pitch_rad;
     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());

ArduCopter/mode_autotune.cpp

@@ -34,9 +34,6 @@ bool AutoTune::init()
 
 void AutoTune::run()
 {
-    // apply SIMPLE mode transform to pilot inputs
-    copter.update_simple_mode();
-
     // disarm when the landing detector says we've landed and spool state is ground idle
     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
         copter.arming.disarm(AP_Arming::Method::LANDED);

ArduCopter/mode_flowhold.cpp

@@ -243,9 +243,6 @@ void ModeFlowHold::run()
     // set vertical speed and acceleration limits
     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
 
-    // apply SIMPLE mode transform to pilot inputs
-    update_simple_mode();
-
     // check for filter change
     if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
         flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());

ArduCopter/mode_land.cpp

@@ -163,9 +163,6 @@ void ModeLand::nogps_run()
 #endif
 
         if (g.land_repositioning) {
-            // apply SIMPLE mode transform to pilot inputs
-            update_simple_mode();
-
             // get pilot desired lean angles
             get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
         }

ArduCopter/mode_loiter.cpp

@@ -10,9 +10,6 @@
 bool ModeLoiter::init(bool ignore_checks)
 {
     float target_roll_rad, target_pitch_rad;
-    // apply SIMPLE mode transform to pilot inputs
-    update_simple_mode();
-
     // convert pilot input to lean angles
     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
 
@@ -86,9 +83,6 @@ void ModeLoiter::run()
     // set vertical speed and acceleration limits
     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
 
-    // apply SIMPLE mode transform to pilot inputs
-    update_simple_mode();
-
     // convert pilot input to lean angles
     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());