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

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)
{
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
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 = 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());
if (rover.mode_auto.mission.num_commands() == 0) {
// add a home location....
add_waypoint_for_current_loc();
}
add_waypoint_for_current_loc();
}
}
break;

View File

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