fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 03:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator land abort use reposition (#7574)

Browse Source
This commit is contained in:
Daniel Agar
2017-08-01 18:59:12 -04:00
committed by GitHub
parent 6bd4e84636
commit 9fb5c4f0e9
7 changed files with 51 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+30 -21
View File
@@ -262,7 +262,7 @@ Mission::find_offboard_land_start()
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
for (size_t i = 0; i < _offboard_mission.count; i++) {
struct mission_item_s missionitem;
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -1120,7 +1120,7 @@ void
Mission::do_abort_landing()
{
// Abort FW landing
// turn the land waypoint into a loiter and stay there
// first climb out then loiter over intended landing location
if (_mission_item.nav_cmd != NAV_CMD_LAND) {
return;
@@ -1129,39 +1129,48 @@ Mission::do_abort_landing()
// loiter at the larger of MIS_LTRMIN_ALT above the landing point
// or 2 * FW_CLMBOUT_DIFF above the current altitude
float alt_landing = get_absolute_altitude_for_item(_mission_item);
float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get());
float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), min_climbout);
// ignore _param_loiter_min_alt if smaller then 0 (-1)
float alt_sp;
if (_param_loiter_min_alt.get() > 0.0f) {
alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(),
_navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
} else {
alt_sp = math::max(alt_landing, _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
}
// turn current landing waypoint into an indefinite loiter
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = alt_sp;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.", (int)(alt_sp - alt_landing));
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
(int)(alt_sp - alt_landing));
// move mission index back 1 (landing approach point) so that re-entering
// the mission doesn't try to land from the loiter above land
// TODO: reset index to MAV_CMD_DO_LAND_START
_current_offboard_mission_index -= 1;
// reset mission index to start of landing
int land_start_index = find_offboard_land_start();
if (land_start_index != -1) {
_current_offboard_mission_index = land_start_index;
} else {
// move mission index back (landing approach point)
_current_offboard_mission_index -= 1;
}
// send reposition cmd to get out of mission
vehicle_command_s vcmd = {};
mission_item_to_vehicle_command(&_mission_item, &vcmd);
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.param1 = -1;
vcmd.param2 = 1;
vcmd.param5 = _mission_item.lat;
vcmd.param6 = _mission_item.lon;
vcmd.param7 = alt_sp;
_navigator->publish_vehicle_cmd(vcmd);
}
bool