diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 24d4846725..a80946c22d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1466,15 +1466,12 @@ FixedwingPositionControl::handle_command() { if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { // only abort landing before point of no return (horizontal and vertical) - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (_control_mode.flag_control_auto_enabled && + _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - if (_land_noreturn_vertical) { - mavlink_log_info(&_mavlink_log_pub, "Landing, can't abort after flare"); - - } else { - _fw_pos_ctrl_status.abort_landing = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, aborted"); - } + _fw_pos_ctrl_status.abort_landing = true; + mavlink_log_critical(&_mavlink_log_pub, "Landing, aborted"); } } } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 6d91542d43..d4ff03b9da 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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 diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 98585cdda5..b60dcf3550 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -509,12 +509,12 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) item->nav_cmd == NAV_CMD_VTOL_LAND; } -void +bool MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { /* don't change the setpoint for non-position items */ if (!item_contains_position(item)) { - return; + return false; } sp->lat = item->lat; @@ -600,6 +600,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } sp->valid = true; + + return sp->valid; } void diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 56f18ac173..b14d00c77f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -86,7 +86,7 @@ protected: * @param the mission item to convert * @param the position setpoint that needs to be set */ - void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + bool mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** * Set previous position setpoint to current setpoint diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0ad16bc560..9ce8b9f9f9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -88,7 +88,7 @@ public: * * @return OK on success. */ - int start(); + int start(); /** * Display the navigator status. diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0c8dc06472..57e32ca697 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -481,15 +481,15 @@ Navigator::task_main() int land_start = _mission.find_offboard_land_start(); if (land_start != -1) { - vehicle_command_s vcmd = {}; - vcmd.target_system = get_vstatus()->system_id; - vcmd.target_component = get_vstatus()->component_id; - vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; - vcmd.param1 = land_start; - vcmd.param2 = 0; + vehicle_command_s cmd_mission_start = {}; + cmd_mission_start.timestamp = hrt_absolute_time(); + cmd_mission_start.target_system = get_vstatus()->system_id; + cmd_mission_start.target_component = get_vstatus()->component_id; + cmd_mission_start.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; + cmd_mission_start.param1 = land_start; + cmd_mission_start.param2 = 0; - vcmd.timestamp = hrt_absolute_time(); - publish_vehicle_cmd(vcmd); + publish_vehicle_cmd(cmd_mission_start); } else { PX4_WARN("planned landing not available"); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 804028256e..94fa1d530e 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -75,7 +75,7 @@ private: void advance_rtl(); /** - * Get rtl altitude + * Get RTL altitude */ float get_rtl_altitude(); @@ -89,7 +89,7 @@ private: RTL_STATE_LOITER, RTL_STATE_LAND, RTL_STATE_LANDED, - } _rtl_state; + } _rtl_state{RTL_STATE_NONE}; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_min_loiter_alt;