Rover: move handling of RC Switches into RC_Channel

Rover: use base-class auxsw handling

Rover: factor out do_aux_function_change_mode

Rover: move mode number enumeration to be in Mode:: namespace

Rover: move mode switch handling to RC_Channel

Rover: rename control_modes.cpp to RC_Channel.cpp

Rover: move motor_active() to be a method on the motors class

APMrover2/APMrover2.cpp

@@ -62,8 +62,8 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(gcs_retry_deferred,     50,   1000),
     SCHED_TASK(gcs_update,             50,   1000),
     SCHED_TASK(gcs_data_stream_send,   50,   3000),
-    SCHED_TASK(read_control_switch,     7,    200),
+    SCHED_TASK(read_mode_switch,        7,    200),
-    SCHED_TASK(read_aux_switch,        10,    200),
+    SCHED_TASK(read_aux_all,           10,    200),
     SCHED_TASK_CLASS(AP_BattMonitor,      &rover.battery,          read,           10,  300),
     SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events,  50,  200),
 #if MOUNT == ENABLED
@@ -97,6 +97,16 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
 #endif
 };
 
+void Rover::read_mode_switch()
+{
+    rover.g2.rc_channels.read_mode_switch();
+}
+
+void Rover::read_aux_all()
+{
+    rover.g2.rc_channels.read_aux_all();
+}
+
 constexpr int8_t Rover::_failsafe_priorities[7];
 
 #if STATS_ENABLED == ENABLED

APMrover2/Parameters.cpp

@@ -109,13 +109,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(pivot_turn_angle,   "PIVOT_TURN_ANGLE",  60),
 
-    // @Param: CH7_OPTION
-    // @DisplayName: Channel 7 option
-    // @Description: What to do use channel 7 for
-    // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm,4:Manual,5:Acro,6:Steering,7:Hold,8:Auto,9:RTL,10:SmartRTL,11:Guided,12:Loiter,13:Follow
-    // @User: Standard
-    GSCALAR(ch7_option,             "CH7_OPTION",          CH7_OPTION),
-
     // @Param: CRUISE_THROTTLE
     // @DisplayName: Base throttle percentage in auto
     // @Description: The base throttle percentage to use in auto mode. The CRUISE_SPEED parameter controls the target speed, but the rover starts with the CRUISE_THROTTLE setting as the initial estimate for how much throttle is needed to achieve that speed. It then adjusts the throttle based on how fast the rover is actually going.
@@ -210,12 +203,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(rangefinder_debounce,   "RNGFND_DEBOUNCE",    2),
 
-    // @Param: AUX_CH
-    // @DisplayName: Auxiliary switch channel
-    // @Description: RC Channel to use for auxiliary functions including saving waypoints
-    // @User: Advanced
-    GSCALAR(aux_channel,    "AUX_CH",       7),
-
     // @Param: MODE_CH
     // @DisplayName: Mode channel
     // @Description: RC Channel to use for driving mode control
@@ -451,7 +438,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 
     // @Group: RC
     // @Path: ../libraries/RC_Channel/RC_Channels.cpp
-    AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels),
+    AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels_Rover),
 
 #if ADVANCED_FAILSAFE == ENABLED
     // @Group: AFS_

APMrover2/Parameters.h

@@ -2,6 +2,8 @@
 
 #include <AP_Common/AP_Common.h>
 
+#include "RC_Channel.h"
+
 // Global parameter class.
 //
 class Parameters {
@@ -159,7 +161,7 @@ public:
         k_param_mode4,
         k_param_mode5,
         k_param_mode6,
-        k_param_aux_channel,
+        k_param_aux_channel_old,
 
         //
         // 220: Waypoint data
@@ -261,7 +263,6 @@ public:
     AP_Int8     mode4;
     AP_Int8     mode5;
     AP_Int8     mode6;
-    AP_Int8     aux_channel;
 
     // Waypoints
     //
@@ -290,7 +291,7 @@ public:
     AP_Int8 sysid_enforce;
 
     // RC input channels
-    RC_Channels rc_channels;
+    RC_Channels_Rover rc_channels;
 
     // control over servo output ranges
     SRV_Channels servo_channels;

APMrover2/RC_Channel.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <RC_Channel/RC_Channel.h>
+#include "Rover.h"
+#include "mode.h"
+
+class RC_Channel_Rover : public RC_Channel
+{
+
+public:
+
+protected:
+
+    void init_aux_function(aux_func_t ch_option, aux_switch_pos_t) override;
+    void do_aux_function(aux_func_t ch_option, aux_switch_pos_t) override;
+
+    // called when the mode switch changes position:
+    void mode_switch_changed(modeswitch_pos_t new_pos) override;
+
+private:
+
+    void do_aux_function_change_mode(Mode &mode,
+                                     const aux_switch_pos_t ch_flag);
+
+};
+
+class RC_Channels_Rover : public RC_Channels
+{
+    bool has_valid_input() const override;
+
+    int8_t flight_mode_channel_number() const override;
+
+public:
+
+    RC_Channel_Rover obj_channels[NUM_RC_CHANNELS];
+    RC_Channel_Rover *channel(const uint8_t chan) override {
+        if (chan > NUM_RC_CHANNELS) {
+            return nullptr;
+        }
+        return &obj_channels[chan];
+    }
+
+};

APMrover2/Rover.h

@@ -73,7 +73,6 @@
 #include <Filter/Filter.h>                          // Filter library
 #include <Filter/LowPassFilter.h>
 #include <Filter/ModeFilter.h>                      // Mode Filter from Filter library
-#include <RC_Channel/RC_Channel.h>                  // RC Channel Library
 #include <StorageManager/StorageManager.h>
 #include <AC_Fence/AC_Fence.h>
 #include <AP_Proximity/AP_Proximity.h>
@@ -96,6 +95,7 @@
 #include "Parameters.h"
 #include "GCS_Mavlink.h"
 #include "GCS_Rover.h"
+#include "RC_Channel.h"                  // RC Channel Library
 
 class Rover : public AP_HAL::HAL::Callbacks {
 public:
@@ -119,6 +119,9 @@ public:
     friend class ModeSmartRTL;
     friend class ModeFollow;
 
+    friend class RC_Channel_Rover;
+    friend class RC_Channels_Rover;
+
     Rover(void);
 
     // HAL::Callbacks implementation.
@@ -168,6 +171,7 @@ private:
 
     // flight modes convenience array
     AP_Int8 *modes;
+    const uint8_t num_modes = 6;
 
     // Inertial Navigation EKF
 #if AP_AHRS_NAVEKF_AVAILABLE
@@ -209,6 +213,9 @@ private:
     GCS_Rover _gcs;  // avoid using this; use gcs()
     GCS_Rover &gcs() { return _gcs; }
 
+    // RC Channels:
+    RC_Channels_Rover &rc() { return g2.rc_channels; }
+
     // relay support
     AP_Relay relay;
 
@@ -280,9 +287,6 @@ private:
     // The amount current ground speed is below min ground speed.  meters per second
     float ground_speed;
 
-    // CH7 auxiliary switches last known position
-    aux_switch_pos aux_ch7;
-
     // Battery Sensors
     AP_BattMonitor battery{MASK_LOG_CURRENT,
                            FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),
@@ -428,14 +432,6 @@ private:
 
     // control_modes.cpp
     Mode *mode_from_mode_num(enum Mode::Number num);
-    void read_control_switch();
-    uint8_t readSwitch(void);
-    void reset_control_switch();
-    aux_switch_pos read_aux_switch_pos();
-    void init_aux_switch();
-    void do_aux_function_change_mode(Mode &mode,
-                                     const aux_switch_pos ch_flag);
-    void read_aux_switch();
 
     // crash_check.cpp
     void crash_check();
@@ -532,6 +528,8 @@ private:
     bool arm_motors(AP_Arming::ArmingMethod method);
     bool disarm_motors(void);
     bool is_boat() const;
+    void read_mode_switch();
+    void read_aux_all();
 
     enum Failsafe_Action {
         Failsafe_Action_None          = 0,

APMrover2/control_modes.cpp

@@ -1,6 +1,14 @@
 #include "Rover.h"
 
-static const int16_t CH_7_PWM_TRIGGER = 1800;
+#include "RC_Channel.h"
+
+// defining these two macros and including the RC_Channels_VarInfo
+// header defines the parameter information common to all vehicle
+// types
+#define RC_CHANNELS_SUBCLASS RC_Channels_Rover
+#define RC_CHANNEL_SUBCLASS RC_Channel_Rover
+
+#include <RC_Channel/RC_Channels_VarInfo.h>
 
 Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
 {
@@ -45,231 +53,184 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     return ret;
 }
 
-void Rover::read_control_switch()
+int8_t RC_Channels_Rover::flight_mode_channel_number() const
 {
-    static bool switch_debouncer;
+    return rover.g.mode_channel;
-    const uint8_t switchPosition = readSwitch();
+}
 
-    // If switchPosition = 255 this indicates that the mode control channel input was out of range
+void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
-    // If we get this value we do not want to change modes.
+{
-    if (switchPosition == 255) {
+    if (new_pos < 0 || (uint8_t)new_pos > rover.num_modes) {
+        // should not have been called
         return;
     }
-
+    Mode *new_mode = rover.mode_from_mode_num((Mode::Number)rover.modes[new_pos].get());
-    if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 100) {
+    if (new_mode != nullptr) {
-        // only use signals that are less than 0.1s old.
+        rover.set_mode(*new_mode, MODE_REASON_TX_COMMAND);
-        return;
     }
-
-    // we look for changes in the switch position. If the
-    // RST_SWITCH_CH parameter is set, then it is a switch that can be
-    // used to force re-reading of the control switch. This is useful
-    // when returning to the previous mode after a failsafe or fence
-    // breach. This channel is best used on a momentary switch (such
-    // as a spring loaded trainer switch).
-    if (oldSwitchPosition != switchPosition ||
-        (g.reset_switch_chan != 0 &&
-         RC_Channels::get_radio_in(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
-        if (switch_debouncer == false) {
-            // this ensures that mode switches only happen if the
-            // switch changes for 2 reads. This prevents momentary
-            // spikes in the mode control channel from causing a mode
-            // switch
-            switch_debouncer = true;
-            return;
-        }
-
-        Mode *new_mode = mode_from_mode_num((enum Mode::Number)modes[switchPosition].get());
-        if (new_mode != nullptr) {
-            set_mode(*new_mode, MODE_REASON_TX_COMMAND);
-        }
-
-        oldSwitchPosition = switchPosition;
-    }
-
-    switch_debouncer = false;
 }
 
-uint8_t Rover::readSwitch(void) {
+// init_aux_switch_function - initialize aux functions
-    const uint16_t pulsewidth = RC_Channels::get_radio_in(g.mode_channel - 1);
+void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
-    if (pulsewidth <= 900 || pulsewidth >= 2200) {
-        return 255;  // This is an error condition
-    }
-    if (pulsewidth <= 1230) {
-        return 0;
-    }
-    if (pulsewidth <= 1360) {
-        return 1;
-    }
-    if (pulsewidth <= 1490) {
-        return 2;
-    }
-    if (pulsewidth <= 1620) {
-        return 3;
-    }
-    if (pulsewidth <= 1749) {
-        return 4;  // Software Manual
-    }
-    return 5;  // Hardware Manual
-}
-
-void Rover::reset_control_switch()
 {
-    oldSwitchPosition = 254;
+    // init channel options
-    read_control_switch();
+    switch(ch_option) {
+        // the following functions do not need initialising:
+    case SAVE_WP:
+    case LEARN_CRUISE:
+    case ARMDISARM:
+    case MANUAL:
+    case ACRO:
+    case STEERING:
+    case HOLD:
+    case AUTO:
+    case GUIDED:
+    case LOITER:
+    case FOLLOW:
+        break;
+    default:
+        RC_Channel::init_aux_function(ch_option, ch_flag);
+        break;
+    }
 }
 
-// ready auxiliary switch's position
+
-aux_switch_pos Rover::read_aux_switch_pos()
+bool RC_Channels_Rover::has_valid_input() const
 {
-    const uint16_t radio_in = channel_aux->get_radio_in();
+    if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
-    if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW;
+        return false;
-    if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH;
+    }
-    return AUX_SWITCH_MIDDLE;
+    if (rover.failsafe.bits & FAILSAFE_EVENT_RC) {
+        return false;
+    }
+    return true;
 }
 
-// initialise position of auxiliary switch
+void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
-void Rover::init_aux_switch()
+                                                    const aux_switch_pos_t ch_flag)
-{
-    aux_ch7 = read_aux_switch_pos();
-}
-
-void Rover::do_aux_function_change_mode(Mode &mode,
-                                        const aux_switch_pos ch_flag)
 {
     switch(ch_flag) {
-    case AUX_SWITCH_HIGH:
+    case HIGH:
-        set_mode(mode, MODE_REASON_TX_COMMAND);
+        rover.set_mode(mode, MODE_REASON_TX_COMMAND);
         break;
-    case AUX_SWITCH_MIDDLE:
+    case MIDDLE:
         // do nothing
         break;
-    case AUX_SWITCH_LOW:
+    case LOW:
-        if (control_mode == &mode) {
+        if (rover.control_mode == &mode) {
-            reset_control_switch();
+            rc().reset_mode_switch();
         }
     }
 }
 
-// read ch7 aux switch
+void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
-void Rover::read_aux_switch()
 {
-    // do not consume input during rc or throttle failsafe
+    switch (ch_option) {
-    if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (failsafe.bits & FAILSAFE_EVENT_RC)) {
+    case DO_NOTHING:
-        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:
+    case SAVE_WP:
-        if (aux_ch7 == AUX_SWITCH_HIGH) {
+        if (ch_flag == HIGH) {
             // do nothing if in AUTO mode
-            if (control_mode == &mode_auto) {
+            if (rover.control_mode == &rover.mode_auto) {
                 return;
             }
 
             // if disarmed clear mission and set home to current location
-            if (!arming.is_armed()) {
+            if (!rover.arming.is_armed()) {
-                mission.clear();
+                rover.mission.clear();
-                set_home_to_current_location(false);
+                rover.set_home_to_current_location(false);
                 return;
             }
 
             // record the waypoint if not in auto mode
-            if (control_mode != &mode_auto) {
+            if (rover.control_mode != &rover.mode_auto) {
                 // create new mission command
                 AP_Mission::Mission_Command cmd = {};
 
                 // set new waypoint to current location
-                cmd.content.location = current_loc;
+                cmd.content.location = rover.current_loc;
 
                 // make the new command to a waypoint
                 cmd.id = MAV_CMD_NAV_WAYPOINT;
 
                 // save command
-                if (mission.add_cmd(cmd)) {
+                if (rover.mission.add_cmd(cmd)) {
-                    hal.console->printf("Added waypoint %u", unsigned(mission.num_commands()));
+                    hal.console->printf("Added waypoint %u", static_cast<uint32_t>(rover.mission.num_commands()));
                 }
             }
         }
         break;
 
     // learn cruise speed and throttle
-    case CH7_LEARN_CRUISE:
+    case LEARN_CRUISE:
-        if (aux_ch7 == AUX_SWITCH_HIGH) {
+        if (ch_flag == HIGH) {
-            cruise_learn_start();
+            rover.cruise_learn_start();
-        } else if (aux_ch7 == AUX_SWITCH_LOW) {
+        } else if (ch_flag == LOW) {
-            cruise_learn_complete();
+            rover.cruise_learn_complete();
         }
         break;
 
     // arm or disarm the motors
-    case CH7_ARM_DISARM:
+    case ARMDISARM:
-        if (aux_ch7 == AUX_SWITCH_HIGH) {
+        if (ch_flag == HIGH) {
-            arm_motors(AP_Arming::RUDDER);
+            rover.arm_motors(AP_Arming::RUDDER);
-        } else if (aux_ch7 == AUX_SWITCH_LOW) {
+        } else if (ch_flag == LOW) {
-            disarm_motors();
+            rover.disarm_motors();
         }
         break;
 
     // set mode to Manual
-    case CH7_MANUAL:
+    case MANUAL:
-        do_aux_function_change_mode(rover.mode_manual, aux_ch7);
+        do_aux_function_change_mode(rover.mode_manual, ch_flag);
         break;
 
     // set mode to Acro
-    case CH7_ACRO:
+    case ACRO:
-        do_aux_function_change_mode(rover.mode_acro, aux_ch7);
+        do_aux_function_change_mode(rover.mode_acro, ch_flag);
         break;
 
     // set mode to Steering
-    case CH7_STEERING:
+    case STEERING:
-        do_aux_function_change_mode(rover.mode_steering, aux_ch7);
+        do_aux_function_change_mode(rover.mode_steering, ch_flag);
         break;
 
     // set mode to Hold
-    case CH7_HOLD:
+    case HOLD:
-        do_aux_function_change_mode(rover.mode_hold, aux_ch7);
+        do_aux_function_change_mode(rover.mode_hold, ch_flag);
         break;
 
     // set mode to Auto
-    case CH7_AUTO:
+    case AUTO:
-        do_aux_function_change_mode(rover.mode_auto, aux_ch7);
+        do_aux_function_change_mode(rover.mode_auto, ch_flag);
         break;
 
     // set mode to RTL
-    case CH7_RTL:
+    case RTL:
-        do_aux_function_change_mode(rover.mode_rtl, aux_ch7);
+        do_aux_function_change_mode(rover.mode_rtl, ch_flag);
         break;
 
     // set mode to SmartRTL
-    case CH7_SMART_RTL:
+    case SMART_RTL:
-        do_aux_function_change_mode(rover.mode_smartrtl, aux_ch7);
+        do_aux_function_change_mode(rover.mode_smartrtl, ch_flag);
         break;
 
     // set mode to Guided
-    case CH7_GUIDED:
+    case GUIDED:
-        do_aux_function_change_mode(rover.mode_guided, aux_ch7);
+        do_aux_function_change_mode(rover.mode_guided, ch_flag);
         break;
 
     // Set mode to LOITER
-    case CH7_LOITER:
+    case LOITER:
-        do_aux_function_change_mode(rover.mode_loiter, aux_ch7);
+        do_aux_function_change_mode(rover.mode_loiter, ch_flag);
         break;
 
     // Set mode to Follow
-    case CH7_FOLLOW:
+    case FOLLOW:
-        do_aux_function_change_mode(rover.mode_follow, aux_ch7);
+        do_aux_function_change_mode(rover.mode_follow, ch_flag);
+        break;
+
+    default:
+        RC_Channel::do_aux_function(ch_option, ch_flag);
         break;
 
     }

APMrover2/defines.h

@@ -13,24 +13,6 @@
 
 #define SERVO_MAX 4500  // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
 
-// CH 7 control
-enum ch7_option {
-    CH7_DO_NOTHING      = 0,
-    CH7_SAVE_WP         = 1,
-    CH7_LEARN_CRUISE    = 2,
-    CH7_ARM_DISARM      = 3,
-    CH7_MANUAL          = 4,
-    CH7_ACRO            = 5,
-    CH7_STEERING        = 6,
-    CH7_HOLD            = 7,
-    CH7_AUTO            = 8,
-    CH7_RTL             = 9,
-    CH7_SMART_RTL       = 10,
-    CH7_GUIDED          = 11,
-    CH7_LOITER          = 12,
-    CH7_FOLLOW          = 13
-};
-
 // HIL enumerations
 #define HIL_MODE_DISABLED 0
 #define HIL_MODE_SENSORS  1
@@ -115,13 +97,6 @@ enum mode_reason_t {
     MODE_REASON_FENCE_BREACH,
 };
 
-// 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
-};
-
 enum pilot_steer_type_t {
     PILOT_STEER_TYPE_DEFAULT = 0,
     PILOT_STEER_TYPE_TWO_PADDLES = 1,

APMrover2/radio.cpp

@@ -6,10 +6,9 @@
 void Rover::set_control_channels(void)
 {
     // check change on RCMAP
-    channel_steer    = RC_Channels::rc_channel(rcmap.roll()-1);
+    channel_steer    = rc().channel(rcmap.roll()-1);
-    channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
+    channel_throttle = rc().channel(rcmap.throttle()-1);
-    channel_aux      = RC_Channels::rc_channel(g.aux_channel-1);
+    channel_lateral  = rc().channel(rcmap.yaw()-1);
-    channel_lateral  = RC_Channels::rc_channel(rcmap.yaw()-1);
 
     // set rc channel ranges
     channel_steer->set_angle(SERVO_MAX);
@@ -104,7 +103,7 @@ void Rover::rudder_arm_disarm_check()
 
 void Rover::read_radio()
 {
-    if (!RC_Channels::read_input()) {
+    if (!rc().read_input()) {
         // check if we lost RC link
         control_failsafe(channel_throttle->get_radio_in());
         return;

APMrover2/system.cpp

@@ -146,11 +146,8 @@ void Rover::init_ardupilot()
     }
     set_mode(*initial_mode, MODE_REASON_INITIALISED);
 
-
+    // initialise rc channels
-    // set the correct flight mode
+    rc().init();
-    // ---------------------------
-    reset_control_switch();
-    init_aux_switch();
 
     // disable safety if requested
     BoardConfig.init_safety();