Copter: add SPORT mode

ArduCopter/ArduCopter.pde

@@ -1637,6 +1637,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
         case ROLL_PITCH_AUTO:
         case ROLL_PITCH_STABLE_OF:
         case ROLL_PITCH_TOY:
+        case ROLL_PITCH_SPORT:
             roll_pitch_initialised = true;
             break;
 
@@ -1761,6 +1762,18 @@ void update_roll_pitch_mode(void)
         get_stabilize_roll(nav_roll);
         get_stabilize_pitch(nav_pitch);
         break;
+
+    case ROLL_PITCH_SPORT:
+        // apply SIMPLE mode transform
+        if(ap.simple_mode && ap_system.new_radio_frame) {
+            update_simple_mode();
+        }
+        // copy user input for reporting purposes
+        control_roll = g.rc_1.control_in;
+        control_pitch = g.rc_2.control_in;
+        get_roll_rate_stabilized_ef(g.rc_1.control_in);
+        get_pitch_rate_stabilized_ef(g.rc_2.control_in);
+        break;
     }
 
 	#if FRAME_CONFIG != HELI_FRAME

ArduCopter/Attitude.pde

@@ -233,6 +233,88 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
     set_yaw_rate_target(0, BODY_EARTH_FRAME);
 }
 
+// Roll with rate input and stabilized in the earth frame
+static void
+get_roll_rate_stabilized_ef(int32_t stick_angle)
+{
+    int32_t angle_error = 0;
+
+    // convert the input to the desired roll rate
+    int32_t target_rate = stick_angle * g.acro_p - (roll_axis * g.acro_balance_roll)/100;
+
+    // convert the input to the desired roll rate
+    roll_axis += target_rate * G_Dt;
+    roll_axis = wrap_180_cd(roll_axis);
+
+    // ensure that we don't reach gimbal lock
+    if (labs(roll_axis) > 4500 && g.acro_trainer_enabled) {
+        roll_axis  = constrain_int32(roll_axis, -4500, 4500);
+        angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
+    } else {
+        // angle error with maximum of +- max_angle_overshoot
+        angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
+        angle_error  = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
+    }
+
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors.motor_runup_complete) {
+        angle_error = 0;
+    }
+#else      
+    // reset target angle to current angle if motors not spinning
+    if (!motors.armed() || g.rc_3.servo_out == 0) {
+        angle_error = 0;
+    }
+#endif // HELI_FRAME
+
+    // update roll_axis to be within max_angle_overshoot of our current heading
+    roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor);
+
+    // set earth frame targets for rate controller
+  set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME);
+}
+
+// Pitch with rate input and stabilized in the earth frame
+static void
+get_pitch_rate_stabilized_ef(int32_t stick_angle)
+{
+    int32_t angle_error = 0;
+
+    // convert the input to the desired pitch rate
+    int32_t target_rate = stick_angle * g.acro_p - (pitch_axis * g.acro_balance_pitch)/100;
+
+    // convert the input to the desired pitch rate
+    pitch_axis += target_rate * G_Dt;
+    pitch_axis = wrap_180_cd(pitch_axis);
+
+    // ensure that we don't reach gimbal lock
+    if (labs(pitch_axis) > 4500) {
+        pitch_axis  = constrain_int32(pitch_axis, -4500, 4500);
+        angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
+    } else {
+        // angle error with maximum of +- max_angle_overshoot
+        angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
+        angle_error  = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
+    }
+
+#if FRAME_CONFIG == HELI_FRAME
+    if (!motors.motor_runup_complete) {
+        angle_error = 0;
+    }
+#else       
+    // reset target angle to current angle if motors not spinning
+    if (!motors.armed() || g.rc_3.servo_out == 0) {
+        angle_error = 0;
+    }
+#endif // HELI_FRAME
+
+    // update pitch_axis to be within max_angle_overshoot of our current heading
+    pitch_axis = wrap_180_cd(angle_error + ahrs.pitch_sensor);
+
+    // set earth frame targets for rate controller
+  set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME);
+}
+
 // Yaw with rate input and stabilized in the earth frame
 static void
 get_yaw_rate_stabilized_ef(int32_t stick_angle)

ArduCopter/Parameters.pde

@@ -308,42 +308,42 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode1, "FLTMODE1",               FLIGHT_MODE_1),
 
     // @Param: FLTMODE2
     // @DisplayName: Flight Mode 2
     // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode2, "FLTMODE2",               FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: Flight Mode 3
     // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode3, "FLTMODE3",               FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: Flight Mode 4
     // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode4, "FLTMODE4",               FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: Flight Mode 5
     // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode5, "FLTMODE5",               FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: Flight Mode 6
     // @Description: Flight mode when Channel 5 pwm is >=1750
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
     // @User: Standard
     GSCALAR(flight_mode6, "FLTMODE6",               FLIGHT_MODE_6),
 

ArduCopter/config.h

@@ -585,6 +585,19 @@
  # define ACRO_THR           	    THROTTLE_MANUAL
 #endif
 
+// Sport Mode
+#ifndef SPORT_YAW
+ # define SPORT_YAW           	    YAW_ACRO
+#endif
+
+#ifndef SPORT_RP
+ # define SPORT_RP            	    ROLL_PITCH_SPORT
+#endif
+
+#ifndef SPORT_THR
+ # define SPORT_THR           	    THROTTLE_MANUAL
+#endif
+
 // Alt Hold Mode
 #ifndef ALT_HOLD_YAW
  # define ALT_HOLD_YAW           	YAW_HOLD

ArduCopter/defines.h

@@ -27,11 +27,12 @@
 #define YAW_RESETTOARMEDYAW				9       // point towards heading at time motors were armed
 
 #define ROLL_PITCH_STABLE           0       // pilot input roll, pitch angles
-#define ROLL_PITCH_ACRO             1       // pilot inputs roll, pitch rotation rates
+#define ROLL_PITCH_ACRO             1       // pilot inputs roll, pitch rotation rates in body frame
 #define ROLL_PITCH_AUTO             2       // no pilot input.  autopilot roll, pitch is sent to stabilize controller inputs
 #define ROLL_PITCH_STABLE_OF        3       // pilot inputs roll, pitch angles which are mixed with optical flow based position controller lean anbles
 #define ROLL_PITCH_TOY              4       // THOR This is the Roll and Pitch mode
 #define ROLL_PITCH_LOITER           5       // pilot inputs the desired horizontal velocities
+#define ROLL_PITCH_SPORT            6       // pilot inputs roll, pitch rotation rates in earth frame
 
 #define THROTTLE_MANUAL                     0   // manual throttle mode - pilot input goes directly to motors
 #define THROTTLE_MANUAL_TILT_COMPENSATED    1   // mostly manual throttle but with some tilt compensation
@@ -135,11 +136,11 @@
 #define CIRCLE 7                        // AUTO control
 #define POSITION 8                      // AUTO control
 #define LAND 9                          // AUTO control
-#define OF_LOITER 10                    // Hold a single location using optical flow
-                                        // sensor
+#define OF_LOITER 10                    // Hold a single location using optical flow sensor
 #define TOY_A 11                        // THOR Enum for Toy mode
 #define TOY_M 12                        // THOR Enum for Toy mode
-#define NUM_MODES 13
+#define SPORT 13                        // earth frame rate control
+#define NUM_MODES 14
 
 // CH_6 Tuning
 // -----------

ArduCopter/events.pde

@@ -15,6 +15,7 @@ static void failsafe_radio_on_event()
     switch(control_mode) {
         case STABILIZE:
         case ACRO:
+        case SPORT:
             // if throttle is zero disarm motors
             if (g.rc_3.control_in == 0) {
                 init_disarm_motors();
@@ -89,6 +90,7 @@ static void low_battery_event(void)
         switch(control_mode) {
             case STABILIZE:
             case ACRO:
+            case SPORT:
                 // if throttle is zero disarm motors
                 if (g.rc_3.control_in == 0) {
                     init_disarm_motors();
@@ -217,6 +219,7 @@ static void failsafe_gcs_check()
     switch(control_mode) {
         case STABILIZE:
         case ACRO:
+        case SPORT:
             // if throttle is zero disarm motors
             if (g.rc_3.control_in == 0) {
                 init_disarm_motors();

ArduCopter/fence.pde

@@ -32,7 +32,7 @@ void fence_check()
 
             // disarm immediately if we think we are on the ground
             // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
-            if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
+            if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
                 init_disarm_motors();
             }else{
                 // if we have a GPS

ArduCopter/motors.pde

@@ -18,8 +18,8 @@ static void arm_motors_check()
         return;
     }
 
-    // allow arming/disarming in ACRO, STABILIZE and TOY flight modes
-    if (control_mode == ACRO || control_mode == STABILIZE || control_mode == TOY_A || control_mode == TOY_M) {
+    // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and TOY
+    if (manual_flight_mode(control_mode)) {
         allow_arming = true;
     }
 
@@ -97,7 +97,7 @@ static void auto_disarm_check()
 {
     static uint8_t auto_disarming_counter;
 
-    if((control_mode <= ACRO) && (g.rc_3.control_in == 0) && motors.armed()) {
+    if(manual_flight_mode(control_mode) && (g.rc_3.control_in == 0) && motors.armed()) {
         auto_disarming_counter++;
 
         if(auto_disarming_counter == AUTO_DISARMING_DELAY) {

ArduCopter/system.pde

@@ -342,9 +342,25 @@ static bool mode_requires_GPS(uint8_t mode) {
     return false;
 }
 
+// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch and yaw controlled by pilot)
+static bool manual_flight_mode(uint8_t mode) {
+    switch(mode) {
+        case ACRO:
+        case STABILIZE:
+        case TOY_A:
+        case TOY_M:
+        case SPORT:
+            return true;
+        default:
+            return false;
+    }
+
+    return false;
+}
+
 // set_mode - change flight mode and perform any necessary initialisation
 // returns true if mode was succesfully set
-// STABILIZE, ACRO and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
+// STABILIZE, ACRO, SPORT and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
 static bool set_mode(uint8_t mode)
 {
     // boolean to record if flight mode could be set
@@ -505,6 +521,16 @@ static bool set_mode(uint8_t mode)
             set_throttle_mode(THROTTLE_HOLD);
             break;
 
+        case SPORT:
+            success = true;
+            ap.manual_throttle = true;
+            ap.manual_attitude = true;
+            set_yaw_mode(SPORT_YAW);
+            set_roll_pitch_mode(SPORT_RP);
+            set_throttle_mode(SPORT_THR);
+            set_nav_mode(NAV_NONE);
+            break;
+
         default:
             success = false;
             break;
@@ -547,7 +573,7 @@ static void update_auto_armed()
             return;
         }
         // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
-        if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
+        if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
             set_auto_armed(false);
         }
     }else{
@@ -668,6 +694,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case TOY_A:
         port->print_P(PSTR("TOY_A"));
         break;
+    case SPORT:
+        port->print_P(PSTR("SPORT"));
+        break;
     default:
         port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
         break;