Rover: aux switch saves waypoint in manual or steering modes

Also refactor aux switch methods
rename LEARN_CH to AUX_CH

APMrover2/APMrover2.cpp

@@ -63,7 +63,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(gcs_update,             50,   1700),
     SCHED_TASK(gcs_data_stream_send,   50,   3000),
     SCHED_TASK(read_control_switch,     7,   1000),
-    SCHED_TASK(read_trim_switch,       10,   1000),
+    SCHED_TASK(read_aux_switch,        10,    100),
     SCHED_TASK(read_battery,           10,   1000),
     SCHED_TASK(read_receiver_rssi,     10,   1000),
     SCHED_TASK(update_events,          50,   1000),

APMrover2/Parameters.cpp

@@ -129,7 +129,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: CH7_OPTION
     // @DisplayName: Channel 7 option
     // @Description: What to do use channel 7 for
-    // @Values: 0:Nothing,1:LearnWaypoint
+    // @Values: 0:Nothing,1:SaveWaypoint
     // @User: Standard
     GSCALAR(ch7_option,             "CH7_OPTION",          CH7_OPTION),
 
@@ -227,11 +227,11 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(rangefinder_debounce,   "RNGFND_DEBOUNCE",    2),
 
-    // @Param: LEARN_CH
+    // @Param: AUX_CH
-    // @DisplayName: Learning channel
+    // @DisplayName: Auxiliary switch channel
-    // @Description: RC Channel to use for learning waypoints
+    // @Description: RC Channel to use for auxiliary functions including saving waypoints
     // @User: Advanced
-    GSCALAR(learn_channel,    "LEARN_CH",       7),
+    GSCALAR(aux_channel,    "AUX_CH",       7),
 
     // @Param: MODE_CH
     // @DisplayName: Mode channel

APMrover2/Parameters.h

@@ -264,7 +264,7 @@ public:
     AP_Int8     mode4;
     AP_Int8     mode5;
     AP_Int8     mode6;
-    AP_Int8     learn_channel;
+    AP_Int8     aux_channel;
 
     // Waypoints
     //

APMrover2/Rover.cpp

@@ -23,7 +23,7 @@ Rover::Rover(void) :
     param_loader(var_info),
     channel_steer(nullptr),
     channel_throttle(nullptr),
-    channel_learn(nullptr),
+    channel_aux(nullptr),
     DataFlash{FIRMWARE_STRING, g.log_bitmask},
     modes(&g.mode1),
     L1_controller(ahrs, nullptr),

APMrover2/Rover.h

@@ -144,7 +144,7 @@ private:
     // primary control channels
     RC_Channel *channel_steer;
     RC_Channel *channel_throttle;
-    RC_Channel *channel_learn;
+    RC_Channel *channel_aux;
 
     DataFlash_Class DataFlash;
 
@@ -276,11 +276,8 @@ private:
     // The amount current ground speed is below min ground speed.  meters per second
     float ground_speed;
 
-    // CH7 control
+    // CH7 auxiliary switches last known position
-    // Used to track the CH7 toggle state.
+    aux_switch_pos aux_ch7;
-    // When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
-    // This allows advanced functionality to know when to execute
-    bool ch7_flag;
 
     // Battery Sensors
     AP_BattMonitor battery;
@@ -448,7 +445,9 @@ private:
     void read_control_switch();
     uint8_t readSwitch(void);
     void reset_control_switch();
-    void read_trim_switch();
+    aux_switch_pos read_aux_switch_pos();
+    void init_aux_switch();
+    void read_aux_switch();
     bool motor_active();
 
     // crash_check.cpp

APMrover2/control_modes.cpp

@@ -110,32 +110,57 @@ void Rover::reset_control_switch()
     read_control_switch();
 }
 
-// read at 10 hz
+// ready auxiliary switch's position
-// set this to your trainer switch
+aux_switch_pos Rover::read_aux_switch_pos()
-void Rover::read_trim_switch()
 {
+    uint16_t radio_in = channel_aux->get_radio_in();
+    if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW;
+    if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH;
+    return AUX_SWITCH_MIDDLE;
+}
+
+// initialise position of auxiliary switch
+void Rover::init_aux_switch()
+{
+    aux_ch7 = read_aux_switch_pos();
+}
+
+// read ch7 aux switch
+void Rover::read_aux_switch()
+{
+    // do not consume input during rc or throttle failsafe
+    if (failsafe.bits & FAILSAFE_EVENT_THROTTLE || failsafe.bits & FAILSAFE_EVENT_RC) {
+        return;
+    }
+
+    // get ch7's current position
+    aux_switch_pos aux_ch7_pos = read_aux_switch_pos();
+
+    // return if no change to switch position
+    if (aux_ch7_pos == aux_ch7) {
+        return;
+    }
+    aux_ch7 = aux_ch7_pos;
+
     switch ((enum ch7_option)g.ch7_option.get()) {
     case CH7_DO_NOTHING:
         break;
     case CH7_SAVE_WP:
-        if (channel_learn->get_radio_in() > CH_7_PWM_TRIGGER) {
+        if (aux_ch7 == AUX_SWITCH_HIGH) {
-            // switch is engaged
+            // do nothing if in AUTO mode
-            ch7_flag = true;
+            if (control_mode == &mode_auto) {
-        } else {  // switch is disengaged
+                return;
-            if (ch7_flag) {
-                ch7_flag = false;
-
-                if (control_mode == &mode_manual) {
-                    hal.console->printf("Erasing waypoints\n");
-                    // if SW7 is ON in MANUAL = Erase the Flight Plan
-                    mission.clear();
-                    if (channel_steer->get_control_in() > 3000) {
-                        // if roll is full right store the current location as home
-                        set_home_to_current_location(false);
             }
-                } else if (control_mode == &mode_learning || control_mode == &mode_steering) {
-                    // if SW7 is ON in LEARNING = record the Wp
 
+            // if disarmed clear mission and set home to current location
+            if (!arming.is_armed()) {
+                mission.clear();
+                set_home_to_current_location(false);
+                return;
+            }
+
+            // record the waypoint if in manual or steering modes
+            if (control_mode == &mode_manual || control_mode == &mode_steering) {
                 // create new mission command
                 AP_Mission::Mission_Command cmd = {};
 
@@ -147,12 +172,7 @@ void Rover::read_trim_switch()
 
                 // save command
                 if (mission.add_cmd(cmd)) {
-                        hal.console->printf("Learning waypoint %u", static_cast<uint32_t>(mission.num_commands()));
+                    hal.console->printf("Added waypoint %u", static_cast<uint32_t>(mission.num_commands()));
-                    }
-                } else if (control_mode == &mode_auto) {
-                    // if SW7 is ON in AUTO = set to RTL
-                    set_mode(mode_rtl, MODE_REASON_TX_COMMAND);
-                    break;
                 }
             }
         }

APMrover2/defines.h

@@ -113,3 +113,13 @@ enum mode_reason_t {
     MODE_REASON_CRASH_FAILSAFE,
     MODE_REASON_MISSION_COMMAND
 };
+
+// values used by the ap.ch7_opt and ap.ch8_opt flags
+enum aux_switch_pos {
+    AUX_SWITCH_LOW,
+    AUX_SWITCH_MIDDLE,
+    AUX_SWITCH_HIGH
+};
+
+#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800   // pwm value above which the ch7 or ch8 option will be invoked
+#define AUX_SWITCH_PWM_TRIGGER_LOW  1200   // pwm value below which the ch7 or ch8 option will be disabled

APMrover2/radio.cpp

@@ -8,7 +8,7 @@ void Rover::set_control_channels(void)
     // check change on RCMAP
     channel_steer    = RC_Channels::rc_channel(rcmap.roll()-1);
     channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
-    channel_learn    = RC_Channels::rc_channel(g.learn_channel-1);
+    channel_aux      = RC_Channels::rc_channel(g.aux_channel-1);
 
     // set rc channel ranges
     channel_steer->set_angle(SERVO_MAX);

APMrover2/system.cpp

@@ -148,6 +148,7 @@ void Rover::init_ardupilot()
     // set the correct flight mode
     // ---------------------------
     reset_control_switch();
+    init_aux_switch();
 
     // disable safety if requested
     BoardConfig.init_safety();