Rover: add Acro mode

ACRO_TURN_RATE allows user control of maximum turn rate
ArduPilot · Nov 29, 2017 · 04e9228 · 04e9228
1 parent 2c6593e
commit 04e9228
Show file tree

Hide file tree

Showing 8 changed files with 84 additions and 9 deletions.
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
@@ -41,7 +41,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: INITIAL_MODE
     // @DisplayName: Initial driving mode
     // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
-    // @Values: 0:MANUAL,2:LEARNING,3:STEERING,4:HOLD,10:AUTO,11:RTL,15:GUIDED
+    // @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,10:AUTO,11:RTL,15:GUIDED
     // @User: Advanced
     GSCALAR(initial_mode,        "INITIAL_MODE",     MANUAL),
 
@@ -241,43 +241,43 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
     GSCALAR(mode1,           "MODE1",         MANUAL),
 
     // @Param: MODE2
     // @DisplayName: Mode2
     // @Description: Driving mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     GSCALAR(mode2,           "MODE2",         MANUAL),
 
     // @Param: MODE3
     // @DisplayName: Mode3
     // @Description: Driving mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     GSCALAR(mode3,           "MODE3",         MANUAL),
 
     // @Param: MODE4
     // @DisplayName: Mode4
     // @Description: Driving mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     GSCALAR(mode4,           "MODE4",         MANUAL),
 
     // @Param: MODE5
     // @DisplayName: Mode5
     // @Description: Driving mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     GSCALAR(mode5,           "MODE5",         MANUAL),
 
     // @Param: MODE6
     // @DisplayName: Mode6
     // @Description: Driving mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         MANUAL),
 
@@ -504,6 +504,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("TURN_RADIUS", 11, ParametersG2, turn_radius, 0.9),
 
+    // @Param: ACRO_TURN_RATE
+    // @DisplayName: Acro mode turn rate maximum
+    // @Description: Acro mode turn rate maximum
+    // @Units: deg/s
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ACRO_TURN_RATE", 12, ParametersG2, acro_turn_rate, 180.0f),
+
     AP_GROUPEND
 };
 

diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
@@ -316,6 +316,9 @@ class ParametersG2 {
 
     // turn radius of vehicle (only used in steering mode)
     AP_Float turn_radius;
+
+    // acro mode turn rate maximum
+    AP_Float acro_turn_rate;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
@@ -104,6 +104,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
 #endif
     friend class GCS_Rover;
     friend class Mode;
+    friend class ModeAcro;
     friend class ModeAuto;
     friend class ModeGuided;
     friend class ModeHold;
@@ -378,6 +379,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
     ModeInitializing mode_initializing;
     ModeHold mode_hold;
     ModeManual mode_manual;
+    ModeAcro mode_acro;
     ModeGuided mode_guided;
     ModeAuto mode_auto;
     ModeSteering mode_steering;

diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp
@@ -9,6 +9,9 @@ Mode *Rover::control_mode_from_num(const enum mode num)
     case MANUAL:
         ret = &mode_manual;
         break;
+    case ACRO:
+        ret = &mode_acro;
+        break;
     case STEERING:
         ret = &mode_steering;
         break;
@@ -156,8 +159,8 @@ void Rover::read_aux_switch()
                 return;
             }
 
-            // record the waypoint if in manual or steering modes
-            if (control_mode == &mode_manual || control_mode == &mode_steering) {
+            // record the waypoint if in manual, acro or steering mode
+            if (control_mode == &mode_manual || control_mode == &mode_acro ||control_mode == &mode_steering) {
                 // create new mission command
                 AP_Mission::Mission_Command cmd = {};
 

diff --git a/APMrover2/defines.h b/APMrover2/defines.h
@@ -28,6 +28,7 @@ enum ch7_option {
 // ----------------
 enum mode {
     MANUAL       = 0,
+    ACRO         = 1,
     STEERING     = 3,
     HOLD         = 4,
     AUTO         = 10,

diff --git a/APMrover2/mode.h b/APMrover2/mode.h
@@ -145,6 +145,20 @@ class Mode
 };
 
 
+class ModeAcro : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return ACRO; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // attributes for mavlink system status reporting
+    bool has_manual_input() const override { return true; }
+};
+
+
 class ModeAuto : public Mode
 {
 public:

diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp
@@ -0,0 +1,40 @@
+#include "mode.h"
+#include "Rover.h"
+
+void ModeAcro::update()
+{
+    float desired_steering, desired_throttle;
+    get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
+
+    // convert pilot throttle input to desired speed (up to twice the cruise speed)
+    float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
+
+    // get speed forward
+    float speed;
+    if (!attitude_control.get_forward_speed(speed)) {
+        // no valid speed so stop
+        g2.motors.set_throttle(0.0f);
+        g2.motors.set_steering(0.0f);
+        return;
+    }
+
+    // determine if pilot is requesting pivot turn
+    bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
+
+    // convert steering request to turn rate in radians/sec
+    float turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
+
+    // set reverse flag backing up
+    bool reversed = is_negative(target_speed);
+    rover.set_reverse(reversed);
+
+    // run speed to throttle output controller
+    if (is_zero(target_speed) && !is_pivot_turning) {
+        stop_vehicle();
+    } else {
+        // run steering turn rate controller and throttle controller
+        float steering_out = attitude_control.get_steering_out_rate(turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
+        g2.motors.set_steering(steering_out * 4500.0f);
+        calc_throttle(target_speed, false);
+    }
+}
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
@@ -290,6 +290,9 @@ void Rover::notify_mode(enum mode new_mode)
     case MANUAL:
         notify.set_flight_mode_str("MANU");
         break;
+    case ACRO:
+        notify.set_flight_mode_str("ACRO");
+        break;
     case STEERING:
         notify.set_flight_mode_str("STER");
         break;