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

Rover: magically set home when toggling first waypoint in 

Currently the first toggle will put home in rather than the first
waypoint
This commit is contained in:
Peter Barker 2018-08-17 12:39:30 +10:00 committed by Randy Mackay
parent e393828702
commit 88575bd536
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
APMrover2/RC_Channel.cpp
View File

@ -78,6 +78,23 @@ void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
} }
} }
void RC_Channel_Rover::add_waypoint_for_current_loc()
{
// create new mission command
AP_Mission::Mission_Command cmd = {};
// set new waypoint to current location
cmd.content.location = rover.current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if (rover.mode_auto.mission.add_cmd(cmd)) {
hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
}
}
void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
{ {
switch (ch_option) { switch (ch_option) {
@ -101,19 +118,11 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
// record the waypoint if not in auto mode // record the waypoint if not in auto mode
if (rover.control_mode != &rover.mode_auto) { if (rover.control_mode != &rover.mode_auto) {
// create new mission command if (rover.mode_auto.mission.num_commands() == 0) {
AP_Mission::Mission_Command cmd = {}; // add a home location....
add_waypoint_for_current_loc();
// set new waypoint to current location
cmd.content.location = rover.current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if (rover.mode_auto.mission.add_cmd(cmd)) {
hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
} }
add_waypoint_for_current_loc();
} }
} }
break; break;

1
APMrover2/RC_Channel.h
View File

@ -22,6 +22,7 @@ private:
void do_aux_function_change_mode(Mode &mode, void do_aux_function_change_mode(Mode &mode,
const aux_switch_pos_t ch_flag); const aux_switch_pos_t ch_flag);
void add_waypoint_for_current_loc();
}; };
class RC_Channels_Rover : public RC_Channels class RC_Channels_Rover : public RC_Channels