Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: aux switch saves waypoint in manual or steering modes 

Also refactor aux switch methods
rename LEARN_CH to AUX_CH
This commit is contained in:
Randy Mackay 2017-08-22 16:06:23 +09:00
parent 3adfb0e77d
commit 524fe4cd82
9 changed files with 79 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.cpp
View File

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

10
APMrover2/Parameters.cpp
View File

@ -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
// @DisplayName: Learning channel
// @Description: RC Channel to use for learning waypoints
// @Param: AUX_CH
// @DisplayName: Auxiliary switch channel
// @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

2
APMrover2/Parameters.h
View File

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

2
APMrover2/Rover.cpp
View File

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

13
APMrover2/Rover.h
View File

@ -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
// Used to track the CH7 toggle state.
// 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;
// CH7 auxiliary switches last known position
aux_switch_pos aux_ch7;
// 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

86
APMrover2/control_modes.cpp
View File

@ -110,49 +110,69 @@ void Rover::reset_control_switch()
read_control_switch();
}
// read at 10 hz
// set this to your trainer switch
void Rover::read_trim_switch()
// ready auxiliary switch's position
aux_switch_pos Rover::read_aux_switch_pos()
{
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) {
// switch is engaged
ch7_flag = true;
} else { // switch is disengaged
if (ch7_flag) {
ch7_flag = false;
if (aux_ch7 == AUX_SWITCH_HIGH) {
// do nothing if in AUTO mode
if (control_mode == &mode_auto) {
return;
}
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;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// 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 = {};
// set new waypoint to current location
cmd.content.location = current_loc;
// set new waypoint to current location
cmd.content.location = current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if (mission.add_cmd(cmd)) {
hal.console->printf("Learning 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;
// save command
if (mission.add_cmd(cmd)) {
hal.console->printf("Added waypoint %u", static_cast<uint32_t>(mission.num_commands()));
}
}
}

10
APMrover2/defines.h
View File

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

2
APMrover2/radio.cpp
View File

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

1
APMrover2/system.cpp
View File

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