Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: move handling of RC Switches into RC_Channel

Browse Source 
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
This commit is contained in:
Peter Barker 2018-05-08 12:35:08 +10:00 committed by Randy Mackay
parent e7e56dde7a
commit 1f0908bba2
9 changed files with 180 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
APMrover2/APMrover2.cpp
View File

@ -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

15
APMrover2/Parameters.cpp
View File

@ -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_

7
APMrover2/Parameters.h
View File

@ -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;

43
APMrover2/RC_Channel.h Normal file
View File

@ -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];
}
};

22
APMrover2/Rover.h
View File

@ -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,

247
APMrover2/control_modes.cpp
View File

@ -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;
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<uint32_t>(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;
}

25
APMrover2/defines.h
View File

@ -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,

9
APMrover2/radio.cpp
View File

@ -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;

7
APMrover2/system.cpp
View File

@ -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();