From c01fabaf119c8bcbeb19457818fa7a6fa664814f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Oct 2020 16:57:12 -0400 Subject: [PATCH] FW move altitude first order hold (FOH) and loiter to position special cases from navigator to position controller Co-authored-by: RomanBapst --- msg/position_controller_status.msg | 2 + .../FixedwingPositionControl.cpp | 112 ++++++++++++++++-- .../FixedwingPositionControl.hpp | 3 + src/modules/navigator/mission.cpp | 90 -------------- src/modules/navigator/mission.h | 16 --- src/modules/navigator/mission_block.cpp | 105 +++------------- src/modules/navigator/mission_params.c | 15 --- 7 files changed, 125 insertions(+), 218 deletions(-) diff --git a/msg/position_controller_status.msg b/msg/position_controller_status.msg index 71b402942d..1ae30ffaff 100644 --- a/msg/position_controller_status.msg +++ b/msg/position_controller_status.msg @@ -14,3 +14,5 @@ float32 acceptance_radius # the optimal distance to a waypoint to switch to the float32 yaw_acceptance # NaN if not set float32 altitude_acceptance # the optimal vertical distance to a waypoint to switch to the next + +uint8 type diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 12e3c83222..6a178a5b1c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -365,6 +365,8 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.timestamp = hrt_absolute_time(); + pos_ctrl_status.type = _type; + _pos_ctrl_status_pub.publish(pos_ctrl_status); } @@ -672,13 +674,97 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 mission_throttle = pos_sp_curr.cruising_throttle; } - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + const float acc_rad = _l1_control.switch_distance(500.0f); + + uint8_t position_sp_type = pos_sp_curr.type; + + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // TAKEOFF: handle like a regular POSITION setpoint if already flying + if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) { + // SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION + position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + + } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION + || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + + float dist_xy = -1.f; + float dist_z = -1.f; + + const float dist = get_distance_to_point_global_wgs84( + (double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt, + _current_latitude, _current_longitude, _current_altitude, + &dist_xy, &dist_z); + + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + // POSITION: achieve position setpoint altitude via loiter + // close to waypoint, but altitude error greater than twice acceptance + if ((dist >= 0.f) + && (dist_z > 2.f * _param_fw_clmbout_diff.get()) + && (dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) { + // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + } else if ((dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) && !pos_sp_next.valid) { + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } + + } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + // LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER + if ((dist >= 0.f) + && (dist_z > 2.f * _param_fw_clmbout_diff.get()) + && (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) { + // SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION + position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + } + } + + _type = position_sp_type; + + + if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust_body[0] = 0.0f; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - /* waypoint is a plain navigation waypoint */ + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + // waypoint is a plain navigation waypoint + float position_sp_alt = pos_sp_curr.alt; + + // Altitude first order hold (FOH) + if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) && + ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || + (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || + (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) + ) { + const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), + pos_sp_prev.lat, pos_sp_prev.lon); + + // Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one + if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { + // Calculate distance to current waypoint + const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), + _current_latitude, _current_longitude); + + // Save distance to waypoint if it is the smallest ever achieved, however make sure that + // _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp + _min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev); + + // if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt + // navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude + if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { + // The setpoint is set linearly and such that the system reaches the current altitude at the acceptance + // radius around the current waypoint + const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt); + const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))); + const float a = pos_sp_prev.alt - grad * d_curr_prev; + + position_sp_alt = a + grad * _min_current_sp_distance_xy; + } + } + } + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d); _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -700,7 +786,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 tecs_fw_mission_throttle = mission_throttle; } - tecs_update_pitch_throttle(now, pos_sp_curr.alt, + tecs_update_pitch_throttle(now, position_sp_alt, calculate_target_airspeed(mission_airspeed, ground_speed), radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()), @@ -711,8 +797,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 radians(_param_fw_p_lim_min.get())); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ float loiter_radius = pos_sp_curr.loiter_radius; uint8_t loiter_direction = pos_sp_curr.loiter_direction; @@ -786,20 +871,20 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 false, radians(_param_fw_p_lim_min.get())); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) { control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); - } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); } /* reset landing state */ - if (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_LAND) { + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1576,7 +1661,12 @@ FixedwingPositionControl::Run() airspeed_poll(); _manual_control_setpoint_sub.update(&_manual_control_setpoint); - _pos_sp_triplet_sub.update(&_pos_sp_triplet); + + if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) { + // reset the altitude foh (first order hold) logic + _min_current_sp_distance_xy = FLT_MAX; + } + vehicle_attitude_poll(); vehicle_command_poll(); vehicle_control_mode_poll(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 63ff81d32e..27e453c15c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -194,6 +194,8 @@ private: float _althold_epv{0.0f}; ///< the position estimate accuracy when engaging alt hold bool _was_in_deadband{false}; ///< wether the last stick input was in althold deadband + float _min_current_sp_distance_xy{FLT_MAX}; + position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies @@ -262,6 +264,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; + uint8_t _type{0}; enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7db1125ec2..59fd34c664 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -222,10 +222,6 @@ Mission::on_active() set_mission_items(); } - } else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) { - - altitude_sp_foh_update(); - } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { @@ -584,9 +580,6 @@ Mission::advance_mission() void Mission::set_mission_items() { - /* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */ - _min_current_sp_distance_xy = FLT_MAX; - /* the home dist check provides user feedback, so we initialize it to this */ bool user_feedback_done = false; @@ -1152,14 +1145,6 @@ Mission::set_mission_items() } } - /* Save the distance between the current sp and the previous one */ - if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { - - _distance_current_previous = get_distance_to_next_waypoint( - pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, - pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon); - } - _navigator->set_position_setpoint_triplet_updated(); } @@ -1354,81 +1339,6 @@ Mission::heading_sp_update() } } -void -Mission::altitude_sp_foh_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* Don't change setpoint if last and current waypoint are not valid - * or if the previous altitude isn't from a position or loiter setpoint or - * if rotary wing since that is handled in the mc_pos_control - */ - - - if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt) - || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) || - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - return; - } - - // Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore - float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); - - if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f); - } - - /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - acc_rad < FLT_EPSILON) { - return; - } - - /* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near - * and the FW controller has a custom landing logic - * - * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon - * */ - if (_mission_item.nav_cmd == NAV_CMD_LAND - || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT - || !_navigator->is_planned_mission()) { - - return; - } - - /* Calculate distance to current waypoint */ - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - /* Save distance to waypoint if it is the smallest ever achieved, however make sure that - * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */ - _min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy), - _distance_current_previous); - - /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt - * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < acc_rad) { - pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item); - - } else { - /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp - * of the mission item as it is used to check if the mission item is reached - * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance - * radius around the current waypoint - **/ - float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt); - float grad = -delta_alt / (_distance_current_previous - acc_rad); - float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous; - pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; - } - - // we set altitude directly so we can run this in parallel to the heading update - _navigator->set_position_setpoint_triplet_updated(); -} - void Mission::cruising_speed_sp_update() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 1c39fbc886..6d4af7266f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -76,11 +76,6 @@ public: void on_activation() override; void on_active() override; - enum mission_altitude_mode { - MISSION_ALTMODE_ZOH = 0, - MISSION_ALTMODE_FOH = 1 - }; - bool set_current_mission_index(uint16_t index); bool land_start(); @@ -155,11 +150,6 @@ private: */ void heading_sp_update(); - /** - * Updates the altitude sp to follow a foh - */ - void altitude_sp_foh_update(); - /** * Update the cruising speed setpoint. */ @@ -243,7 +233,6 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamFloat) _param_mis_dist_wps, - (ParamInt) _param_mis_altmode, (ParamInt) _param_mis_mnt_yaw_ctl ) @@ -272,11 +261,6 @@ private: bool _mission_waypoints_changed{false}; bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ - float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint */ - - float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet, - only use if current and previous are valid */ - enum work_item_type { WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b02ab050b5..c8942e9cf8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -150,40 +150,6 @@ MissionBlock::is_mission_item_reached() _navigator->get_global_position()->alt, &dist_xy, &dist_z); - /* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */ - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && - (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { - - struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; - - /* close to waypoint, but altitude error greater than twice acceptance */ - if ((dist >= 0.0f) - && (dist_z > 2 * _navigator->get_altitude_acceptance_radius()) - && (dist_xy < 2 * _navigator->get_loiter_radius())) { - - /* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */ - if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; - curr_sp->loiter_radius = _navigator->get_loiter_radius(); - curr_sp->loiter_direction = 1; - _navigator->set_position_setpoint_triplet_updated(); - } - - } else { - /* restore SETPOINT_TYPE_POSITION */ - if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - /* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */ - if ((dist >= 0.0f) - && (dist_z < _navigator->get_loiter_radius()) - && (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) { - - curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _navigator->set_position_setpoint_triplet_updated(); - } - } - } - } - if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { @@ -218,6 +184,7 @@ MissionBlock::is_mission_item_reached() } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. @@ -239,50 +206,24 @@ MissionBlock::is_mission_item_reached() } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { + if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) + && dist_z <= _navigator->get_altitude_acceptance_radius()) { - // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter - // first check if the altitude setpoint is the mission setpoint - struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + _waypoint_position_reached = true; - if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { - // check if the initial loiter has been accepted - dist_xy = -1.0f; - dist_z = -1.0f; + // set required yaw from bearing to the next mission item + if (_mission_item.force_heading) { + const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); + if (next_sp.valid) { + _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + next_sp.lat, next_sp.lon); - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_default_altitude_acceptance_radius()) { + _waypoint_yaw_reached = false; - // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item - curr_sp->alt = altitude_amsl; - _navigator->set_position_setpoint_triplet_updated(); - } - - } else { - if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { - - _waypoint_position_reached = true; - - // set required yaw from bearing to the next mission item - if (_mission_item.force_heading) { - const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; - - if (next_sp.valid) { - _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - next_sp.lat, next_sp.lon); - - _waypoint_yaw_reached = false; - - } else { - _waypoint_yaw_reached = true; - } + } else { + _waypoint_yaw_reached = true; } } } @@ -365,7 +306,6 @@ MissionBlock::is_mission_item_reached() } /* Check if the waypoint and the requested yaw setpoint. */ - if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING @@ -615,20 +555,13 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; - case NAV_CMD_LOITER_TO_ALT: - - // initially use current altitude, and switch to mission item altitude once in loiter position - if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller than 0 (-1) - sp->alt = math::max(_navigator->get_global_position()->alt, - _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); - - } else { - sp->alt = _navigator->get_global_position()->alt; - } - - // fall through case NAV_CMD_LOITER_TIME_LIMIT: + + // FALLTHROUGH case NAV_CMD_LOITER_UNLIMITED: + + // FALLTHROUGH + case NAV_CMD_LOITER_TO_ALT: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 17e6bba7ab..56ebdd3284 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -114,21 +114,6 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); */ PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900); -/** - * Altitude setpoint mode - * - * 0: the system will follow a zero order hold altitude setpoint - * 1: the system will follow a first order hold altitude setpoint - * values follow the definition in enum mission_altitude_mode - * - * @min 0 - * @max 1 - * @value 0 Zero Order Hold - * @value 1 First Order Hold - * @group Mission - */ -PARAM_DEFINE_INT32(MIS_ALTMODE, 1); - /** * Enable yaw control of the mount. (Only affects multicopters and ROI mission items) *