diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 71507fab18..790a977205 100644 --- a/APMrover2/APMrover2.cpp +++ b/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_aux_switch, 10, 200), + SCHED_TASK(read_mode_switch, 7, 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 diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index c59be72eb6..d72f8b3c7e 100644 --- a/APMrover2/Parameters.cpp +++ b/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_ diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index ca2303b875..4ffae11446 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -2,6 +2,8 @@ #include +#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; diff --git a/APMrover2/RC_Channel.h b/APMrover2/RC_Channel.h new file mode 100644 index 0000000000..b0d4bf3b1f --- /dev/null +++ b/APMrover2/RC_Channel.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#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]; + } + +}; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index dd69ecdd50..de78358954 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -73,7 +73,6 @@ #include // Filter library #include #include // Mode Filter from Filter library -#include // RC Channel Library #include #include #include @@ -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, diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index abd2ae8815..8ddde4ca98 100644 --- a/APMrover2/control_modes.cpp +++ b/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 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; - const uint8_t switchPosition = readSwitch(); + return rover.g.mode_channel; +} - // If switchPosition = 255 this indicates that the mode control channel input was out of range - // If we get this value we do not want to change modes. - if (switchPosition == 255) { +void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos) +{ + if (new_pos < 0 || (uint8_t)new_pos > rover.num_modes) { + // should not have been called return; } - - if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 100) { - // only use signals that are less than 0.1s old. - return; + Mode *new_mode = rover.mode_from_mode_num((Mode::Number)rover.modes[new_pos].get()); + if (new_mode != nullptr) { + rover.set_mode(*new_mode, MODE_REASON_TX_COMMAND); } - - // 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) { - const uint16_t pulsewidth = RC_Channels::get_radio_in(g.mode_channel - 1); - 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() +// init_aux_switch_function - initialize aux functions +void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { - oldSwitchPosition = 254; - read_control_switch(); + // init channel options + 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 (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; + if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { + return false; + } + if (rover.failsafe.bits & FAILSAFE_EVENT_RC) { + return false; + } + return true; } -// initialise position of auxiliary switch -void Rover::init_aux_switch() -{ - aux_ch7 = read_aux_switch_pos(); -} - -void Rover::do_aux_function_change_mode(Mode &mode, - const aux_switch_pos ch_flag) +void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode, + const aux_switch_pos_t ch_flag) { switch(ch_flag) { - case AUX_SWITCH_HIGH: - set_mode(mode, MODE_REASON_TX_COMMAND); + case HIGH: + rover.set_mode(mode, MODE_REASON_TX_COMMAND); break; - case AUX_SWITCH_MIDDLE: + case MIDDLE: // do nothing break; - case AUX_SWITCH_LOW: - if (control_mode == &mode) { - reset_control_switch(); + case LOW: + if (rover.control_mode == &mode) { + rc().reset_mode_switch(); } } } -// read ch7 aux switch -void Rover::read_aux_switch() +void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { - // 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: + switch (ch_option) { + case DO_NOTHING: break; - case CH7_SAVE_WP: - if (aux_ch7 == AUX_SWITCH_HIGH) { + case SAVE_WP: + 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()) { - mission.clear(); - set_home_to_current_location(false); + if (!rover.arming.is_armed()) { + rover.mission.clear(); + 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)) { - hal.console->printf("Added waypoint %u", unsigned(mission.num_commands())); + if (rover.mission.add_cmd(cmd)) { + hal.console->printf("Added waypoint %u", static_cast(rover.mission.num_commands())); } } } break; // learn cruise speed and throttle - case CH7_LEARN_CRUISE: - if (aux_ch7 == AUX_SWITCH_HIGH) { - cruise_learn_start(); - } else if (aux_ch7 == AUX_SWITCH_LOW) { - cruise_learn_complete(); + case LEARN_CRUISE: + if (ch_flag == HIGH) { + rover.cruise_learn_start(); + } else if (ch_flag == LOW) { + rover.cruise_learn_complete(); } break; // arm or disarm the motors - case CH7_ARM_DISARM: - if (aux_ch7 == AUX_SWITCH_HIGH) { - arm_motors(AP_Arming::RUDDER); - } else if (aux_ch7 == AUX_SWITCH_LOW) { - disarm_motors(); + case ARMDISARM: + if (ch_flag == HIGH) { + rover.arm_motors(AP_Arming::RUDDER); + } else if (ch_flag == LOW) { + rover.disarm_motors(); } break; // set mode to Manual - case CH7_MANUAL: - do_aux_function_change_mode(rover.mode_manual, aux_ch7); + case MANUAL: + do_aux_function_change_mode(rover.mode_manual, ch_flag); break; // set mode to Acro - case CH7_ACRO: - do_aux_function_change_mode(rover.mode_acro, aux_ch7); + case ACRO: + do_aux_function_change_mode(rover.mode_acro, ch_flag); break; // set mode to Steering - case CH7_STEERING: - do_aux_function_change_mode(rover.mode_steering, aux_ch7); + case STEERING: + do_aux_function_change_mode(rover.mode_steering, ch_flag); break; // set mode to Hold - case CH7_HOLD: - do_aux_function_change_mode(rover.mode_hold, aux_ch7); + case HOLD: + do_aux_function_change_mode(rover.mode_hold, ch_flag); break; // set mode to Auto - case CH7_AUTO: - do_aux_function_change_mode(rover.mode_auto, aux_ch7); + case AUTO: + do_aux_function_change_mode(rover.mode_auto, ch_flag); break; // set mode to RTL - case CH7_RTL: - do_aux_function_change_mode(rover.mode_rtl, aux_ch7); + case RTL: + do_aux_function_change_mode(rover.mode_rtl, ch_flag); break; // set mode to SmartRTL - case CH7_SMART_RTL: - do_aux_function_change_mode(rover.mode_smartrtl, aux_ch7); + case SMART_RTL: + do_aux_function_change_mode(rover.mode_smartrtl, ch_flag); break; // set mode to Guided - case CH7_GUIDED: - do_aux_function_change_mode(rover.mode_guided, aux_ch7); + case GUIDED: + do_aux_function_change_mode(rover.mode_guided, ch_flag); break; // Set mode to LOITER - case CH7_LOITER: - do_aux_function_change_mode(rover.mode_loiter, aux_ch7); + case LOITER: + do_aux_function_change_mode(rover.mode_loiter, ch_flag); break; // Set mode to Follow - case CH7_FOLLOW: - do_aux_function_change_mode(rover.mode_follow, aux_ch7); + case FOLLOW: + do_aux_function_change_mode(rover.mode_follow, ch_flag); + break; + + default: + RC_Channel::do_aux_function(ch_option, ch_flag); break; } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index bdb0664069..5ec933c5fd 100644 --- a/APMrover2/defines.h +++ b/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, diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 0137e836bb..33e5716399 100644 --- a/APMrover2/radio.cpp +++ b/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_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); - channel_aux = RC_Channels::rc_channel(g.aux_channel-1); - channel_lateral = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_steer = rc().channel(rcmap.roll()-1); + channel_throttle = rc().channel(rcmap.throttle()-1); + channel_lateral = 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; diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 34eb13602c..67eff35764 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -146,11 +146,8 @@ void Rover::init_ardupilot() } set_mode(*initial_mode, MODE_REASON_INITIALISED); - - // set the correct flight mode - // --------------------------- - reset_control_switch(); - init_aux_switch(); + // initialise rc channels + rc().init(); // disable safety if requested BoardConfig.init_safety();