From ea91f4d47b4521a7f7098cb6a9893aa2ed0143b6 Mon Sep 17 00:00:00 2001 From: Michael Day Date: Fri, 17 Oct 2014 10:34:04 -0700 Subject: [PATCH] AP_Mission: Added support for MAV_CMD_DO_LAND_START --- libraries/AP_Mission/AP_Mission.cpp | 43 +++++++++++++++++++++++++++++ libraries/AP_Mission/AP_Mission.h | 4 +++ 2 files changed, 47 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 49458bbe63..4de26e827f 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -589,6 +589,10 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP cmd.content.repeat_servo.cycle_time = packet.param4; // time in seconds break; + case MAV_CMD_DO_LAND_START: // MAV ID: 189 + copy_location = true; + break; + case MAV_CMD_DO_SET_ROI: // MAV ID: 201 copy_location = true; cmd.p1 = packet.param1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) @@ -850,6 +854,10 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, packet.param4 = cmd.content.repeat_servo.cycle_time; // time in milliseconds converted to seconds break; + case MAV_CMD_DO_LAND_START: // MAV ID: 189 + copy_location = true; + break; + case MAV_CMD_DO_SET_ROI: // MAV ID: 201 copy_location = true; packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) @@ -1232,3 +1240,38 @@ uint16_t AP_Mission::num_commands_max(void) const return (_storage.size() - 4) / AP_MISSION_EEPROM_COMMAND_SIZE; } +// find the nearest landing sequence starting point (DO_LAND_START) and +// switch to that mission item. +bool AP_Mission::jump_to_landing_sequence() { + struct Location current_loc; + + if (! ((AP_AHRS&) _ahrs).get_position(current_loc)) { + return false; + } + + int16_t landing_start_index = -1; + float min_distance = 999999999.9; + int tmp_distance; + Mission_Command tmp = {0}; + + // Go through mission looking for nearest landing start command + for(uint16_t i = 0; i < num_commands(); i++) { + read_cmd_from_storage(i, tmp); + if(tmp.id == MAV_CMD_DO_LAND_START) { + read_cmd_from_storage(i, tmp); + tmp_distance = get_distance(tmp.content.location, current_loc); + if(tmp_distance < min_distance) { + min_distance = tmp_distance; + + landing_start_index = i+1; // go to the NEXT mission item, otherwise you will keep restarting preland_init + } + } + } + + if (landing_start_index == -1) { + return false; + } + + return set_current_cmd(landing_start_index); +} + diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index ec13605dc7..7097f1366d 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -320,6 +320,10 @@ public: // return the last time the mission changed in milliseconds uint32_t last_change_time_ms(void) const { return _last_change_time_ms; } + // find the nearest landing sequence starting point (DO_LAND_START) and + // switch to that mission item. + bool jump_to_landing_sequence(); + // user settable parameters static const struct AP_Param::GroupInfo var_info[];