Rover: add simple mode

APMrover2/Parameters.cpp

@@ -33,7 +33,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,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Advanced
     GSCALAR(initial_mode,        "INITIAL_MODE",     Mode::Number::MANUAL),
 
@@ -211,7 +211,7 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
     GSCALAR(mode1,           "MODE1",         Mode::Number::MANUAL),
@@ -219,35 +219,35 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: MODE2
     // @DisplayName: Mode2
     // @Description: Driving mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode2,           "MODE2",         Mode::Number::MANUAL),
 
     // @Param: MODE3
     // @DisplayName: Mode3
     // @Description: Driving mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode3,           "MODE3",         Mode::Number::MANUAL),
 
     // @Param: MODE4
     // @DisplayName: Mode4
     // @Description: Driving mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode4,           "MODE4",         Mode::Number::MANUAL),
 
     // @Param: MODE5
     // @DisplayName: Mode5
     // @Description: Driving mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode5,           "MODE5",         Mode::Number::MANUAL),
 
     // @Param: MODE6
     // @DisplayName: Mode6
     // @Description: Driving mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         Mode::Number::MANUAL),
 
@@ -591,6 +591,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(rally, "RALLY_", 28, ParametersG2, AP_Rally_Rover),
 #endif
 
+    // @Param: SIMPLE_TYPE
+    // @DisplayName: Simple_Type
+    // @Description: Simple mode types
+    // @Values: 0:InitialHeading,1:CardinalDirections
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("SIMPLE_TYPE", 29, ParametersG2, simple_type, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -367,6 +367,9 @@ public:
 
     // Rally point library
     AP_Rally_Rover rally;
+
+    // Simple mode types
+    AP_Int8 simple_type;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/RC_Channel.cpp

@@ -32,6 +32,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::FOLLOW:
         ret = &mode_follow;
         break;
+    case Mode::Number::SIMPLE:
+        ret = &mode_simple;
+        break;
     case Mode::Number::AUTO:
         ret = &mode_auto;
         break;
@@ -226,6 +229,11 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
         do_aux_function_change_mode(rover.mode_follow, ch_flag);
         break;
 
+    // set mode to Simple
+    case SIMPLE:
+        do_aux_function_change_mode(rover.mode_simple, ch_flag);
+        break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

APMrover2/Rover.h

@@ -121,6 +121,7 @@ public:
     friend class ModeRTL;
     friend class ModeSmartRTL;
     friend class ModeFollow;
+    friend class ModeSimple;
 
     friend class RC_Channel_Rover;
     friend class RC_Channels_Rover;
@@ -376,6 +377,7 @@ private:
     ModeRTL mode_rtl;
     ModeSmartRTL mode_smartrtl;
     ModeFollow mode_follow;
+    ModeSimple mode_simple;
 
     // cruise throttle and speed learning
     struct {
@@ -397,6 +399,7 @@ private:
     void one_second_loop(void);
     void update_GPS(void);
     void update_current_mode(void);
+    void init_simple_mode(void);
 
     // balance_bot.cpp
     void balancebot_pitch_control(float &throttle);
@@ -576,6 +579,9 @@ public:
     // frame type
     uint8_t get_frame_type() { return g2.frame_type.get(); }
     AP_WheelRateControl& get_wheel_rate_control() { return g2.wheel_rate_control; }
+
+    // Simple mode
+    float simple_sin_yaw;
 };
 
 extern const AP_HAL::HAL& hal;

APMrover2/mode.cpp

@@ -145,6 +145,30 @@ void Mode::get_pilot_desired_lateral(float &lateral_out)
     lateral_out = rover.channel_lateral->get_control_in();
 }
 
+// decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
+void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out)
+{
+    float desired_steering;
+    float desired_throttle;
+    get_pilot_input(desired_steering, desired_throttle);
+
+    // scale down and limit throttle and steering to a -1 to +1 range
+    desired_throttle = constrain_float(desired_throttle / 100.0f, -1.0f, 1.0f);
+    desired_steering = constrain_float(desired_steering / 4500.0f, -1.0, 1.0f);
+
+    // calculate angle of input stick vector
+    heading_out = wrap_360_cd(atan2f(desired_steering,desired_throttle) * DEGX100);
+
+    // calculate magnitude of input stick vector
+    float magnitude_max = 1.0f;
+    float magnitude = safe_sqrt(sq(desired_throttle) + sq(desired_steering));
+    if (magnitude > magnitude_max) {
+        magnitude = magnitude_max;
+    }
+    float throttle = magnitude / magnitude_max;
+    speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
+}
+
 // set desired location
 void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {

APMrover2/mode.h

@@ -25,6 +25,7 @@ public:
         HOLD         = 4,
         LOITER       = 5,
         FOLLOW       = 6,
+        SIMPLE       = 7,
         AUTO         = 10,
         RTL          = 11,
         SMART_RTL    = 12,
@@ -134,6 +135,9 @@ protected:
     // decode pilot lateral movement input and return in lateral_out argument
     void get_pilot_desired_lateral(float &lateral_out);
 
+    // decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
+    void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out);
+
     // calculate steering output to drive along line from origin to destination waypoint
     void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
 
@@ -482,3 +486,26 @@ protected:
 
     bool _enter() override;
 };
+
+class ModeSimple : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return SIMPLE; }
+    const char *name4() const override { return "SMPL"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+    void init_simple_heading();
+
+protected:
+
+    float simple_initial_heading;
+
+    // simple type enum used for SIMPLE_TYPE parameter
+    enum simple_type {
+        Simple_InitialHeading = 0,
+        Simple_CardinalDirections = 1,
+    };
+};
+

APMrover2/mode_simple.cpp

@@ -0,0 +1,41 @@
+#include "mode.h"
+#include "Rover.h"
+
+void ModeSimple::init_simple_heading()
+{
+    simple_initial_heading = ahrs.yaw;
+}
+
+void ModeSimple::update()
+{
+    float desired_heading, desired_steering, desired_speed;
+
+    // initial heading simple mode
+    if (g2.simple_type == Simple_InitialHeading) {
+
+        // get piot input
+        get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
+
+        float simple_heading;
+        if (is_zero(desired_steering)) {
+            simple_heading = ((simple_initial_heading - ahrs.yaw) * 4500.0f);
+        } else {
+            simple_heading = desired_steering;
+        }
+
+        // run throttle and steering controllers
+        calc_steering_to_heading(simple_heading, false);
+        calc_throttle(desired_speed, true, false);
+    }
+
+    // cardinal directions simple mode
+    if (g2.simple_type == Simple_CardinalDirections) {
+
+        // get pilot input
+        get_pilot_desired_heading_and_speed(desired_heading, desired_speed);
+
+        // run throttle and steering controllers
+        calc_steering_to_heading(desired_heading, false);
+        calc_throttle(desired_speed, false, true);
+    }
+}

APMrover2/system.cpp

@@ -332,6 +332,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
     // Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point
     g2.smart_rtl.set_home(true);
 
+    // initialize simple mode heading
+    rover.mode_simple.init_simple_heading();
+
     change_arm_state();
     return true;
 }