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

Plane: fixed relative altitudes and resuming mission

Browse Source 
Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2014-03-14 13:06:40 +11:00 committed by Randy Mackay
parent 107cc2c126
commit 3277416aeb
5 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/GCS_Mavlink.pde
View File

@ -1606,7 +1606,11 @@ mission_item_send_failed:
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// set current command
if (mission.set_current_cmd(packet.seq)) {
if (packet.seq == 0) {
mission.start();
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
} else if (mission.set_current_cmd(packet.seq)) {
mission.resume();
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
}
break;
@ -1628,12 +1632,7 @@ mission_item_send_failed:
break;
}
// new mission arriving, clear current mission
if (!mission.clear()) {
// return error if we were unable to clear the mission (possibly because we're currently flying the mission)
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
break;
}
mission.truncate(packet.count);
waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;

1
ArduPlane/commands_logic.pde
View File

@ -160,6 +160,7 @@ static void exit_mission()
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = rally_find_best_cmd(current_loc, home);
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
setup_glide_slope();
start_command(next_nav_command);
}

2
ArduPlane/commands_process.pde
View File

@ -11,7 +11,7 @@ static void update_commands(void)
if(home_is_set == true && mission.num_commands() > 1) {
if(mission.state() == AP_Mission::MISSION_RUNNING) {
mission.update();
}else if(mission.state() == AP_Mission::MISSION_COMPLETE) {
} else {
// next_nav_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission
verify_command(next_nav_command);
}

6
ArduPlane/control_modes.pde
View File

@ -48,10 +48,8 @@ static void read_control_switch()
if (g.reset_mission_chan != 0 &&
hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
if (mission.set_current_cmd(0)) {
// reset to first waypoint in mission
prev_WP.content.location = current_loc;
}
mission.start();
prev_WP.content.location = current_loc;
}
switch_debouncer = false;

6
ArduPlane/rally.pde
View File

@ -66,7 +66,9 @@ static bool find_best_rally_point(const Location &myloc, const Location &homeloc
static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
{
Location ret = {};
ret.flags.relative_alt = true;
// we return an absolute altitude, as we add homeloc.alt below
ret.flags.relative_alt = false;
//Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound
@ -92,6 +94,8 @@ static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, co
ret.content.location = homeloc;
// Altitude to hold over home
ret.content.location.alt = read_alt_to_hold();
// read_alt_to_hold returns an absolute altitude
ret.content.location.flags.relative_alt = false;
}
return ret;
}