Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: Added support for MAV_CMD_DO_LAND_START

Browse Source
This commit is contained in:
Michael Day 2014-10-17 10:34:04 -07:00 committed by Andrew Tridgell
parent d62b9a7044
commit ea91f4d47b
2 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_Mission/AP_Mission.cpp
View File

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

4
libraries/AP_Mission/AP_Mission.h
View File

@ -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[];