diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 5786db9da6..60e472bafa 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -206,7 +206,7 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0); * hard altitude limit. This setting will * be consolidated with the GF_MAX_VER_DIST * parameter. - * A negative value indicates that there is no limit + * A negative value indicates no altitude limitation. * * @unit m * @min -1 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f5bb9a8db0..004325a35e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -919,28 +919,30 @@ MulticopterPositionControl::reset_alt_sp() void MulticopterPositionControl::limit_altitude() { - if (_vehicle_land_detected.alt_max > 0.0f) { - /* in altitude control, limit setpoint */ - if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) { - _pos_sp(2) = -_vehicle_land_detected.alt_max; - return; - } + if (_vehicle_land_detected.alt_max < 0.0f) { + // there is no altitude limitation present + return; + } - /* in velocity control mode and want to fly upwards */ - if (!_run_alt_control && _vel_sp(2) <= 0.0f) { + float altitude_above_home = -(_pos(2) - _home_pos.z); - /* time to travel to reach zero velocity */ - float delta_t = -_vel(2) / _acceleration_z_max_down.get(); + if (_run_alt_control && (altitude_above_home > _vehicle_land_detected.alt_max)) { + // we are above maximum altitude + _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; - /* predicted position */ - float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * - _acceleration_z_max_down.get() * delta_t *delta_t; + } else if (!_run_alt_control && _vel_sp(2) <= 0.0f) { + // we want to fly upwards: check if vehicle does not exceed altitude - if (pos_z_next <= -_vehicle_land_detected.alt_max) { - _pos_sp(2) = -_vehicle_land_detected.alt_max; - _run_alt_control = true; - return; - } + // time to reach zero velocity + float delta_t = -_vel(2) / _acceleration_z_max_down.get(); + + // predict next position based on current position, velocity, max acceleration downwards and time to reach zero velocity + float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t; + + if (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max) { + // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint + _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; + _run_alt_control = true; } } } diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 9f71bc6527..a191df00e5 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -126,9 +126,8 @@ EngineFailure::set_ef_item() reset_mission_item_reached(); /* convert mission item to current position setpoint and make it valid */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 182aaa1fbd..c91a9798a7 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -361,7 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa pos_sp_triplet->previous.valid = use_position; pos_sp_triplet->previous = pos_sp_triplet->current; - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; pos_sp_triplet->current.position_valid = use_position; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 6d2908d616..e96913ac8f 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -81,12 +81,13 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + _navigator->set_can_loiter_at_sp(false); + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index eaf9a71f59..aefb6b5d3f 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -129,7 +129,7 @@ Loiter::set_loiter_position() struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->previous.valid = false; - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3e7492927a..86fc07e3e1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -508,7 +508,7 @@ Mission::set_mission_items() /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; @@ -765,7 +765,7 @@ Mission::set_mission_items() set_align_mission_item(&_mission_item, &mission_item_next_position); /* set position setpoint to target during the transition */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); } @@ -808,7 +808,7 @@ Mission::set_mission_items() /*********************************** set setpoints and check next *********************************************/ /* set current position setpoint from mission item (is protected against non-position items) */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); /* issue command if ready (will do nothing for position mission items) */ @@ -835,7 +835,7 @@ Mission::set_mission_items() /* try to process next mission item */ if (has_next_position_item) { /* got next mission item, update setpoint triplet */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); } else { @@ -1187,7 +1187,7 @@ Mission::do_abort_landing() _mission_item.origin = ORIGIN_ONBOARD; struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 55c565f4dd..c288aad6ed 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -751,22 +751,30 @@ MissionBlock::set_idle_item(struct mission_item_s *item) } void -MissionBlock::mission_apply_limitation(struct mission_item_s *item) +MissionBlock::mission_apply_limitation(mission_item_s &item) { + /* + * Limit altitude + */ /* do nothing if altitude max is negative */ if (_navigator->get_land_detected()->alt_max > 0.0f) { - /* get absolut altitude */ - float altitude_abs = item->altitude_is_relative - ? item->altitude + _navigator->get_home_position()->alt - : item->altitude; + /* absolute altitude */ + float altitude_abs = item.altitude_is_relative + ? item.altitude + _navigator->get_home_position()->alt + : item.altitude; + /* limit altitude to maximum allowed altitude */ if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) { - item->altitude = item->altitude_is_relative ? - _navigator->get_land_detected()->alt_max : - _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt; + item.altitude = item.altitude_is_relative ? + _navigator->get_land_detected()->alt_max : + _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt; } } + + /* + * Add other limitations here + */ } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 893b9ab2c2..ab464478e1 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -121,10 +121,9 @@ protected: void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw); /** - * This function limits the setpoint dependent - * on vehicle model - */ - void mission_apply_limitation(struct mission_item_s *item); + * General function used to adjust the mission item based on vehicle specific limitations + */ + void mission_apply_limitation(mission_item_s &item); void issue_command(const mission_item_s &item); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7fa4b6047b..0a992b865b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -36,7 +36,6 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler - * @author Dennis Mannhart */ #ifndef NAVIGATOR_H diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 54fe45c1ce..92f679b9a1 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -140,7 +140,7 @@ RCLoss::set_rcl_item() reset_mission_item_reached(); /* convert mission item to current position setpoint and make it valid */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 3b135a0f37..9a8b9e8158 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -84,7 +84,7 @@ RTL::on_activation() { set_current_position_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; @@ -292,7 +292,7 @@ RTL::set_rtl_item() } /* convert mission item to current position setpoint and make it valid */ - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 2ed63d0d80..94fa1d530e 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -79,6 +79,7 @@ private: */ float get_rtl_altitude(); + enum RTLState { RTL_STATE_NONE = 0, RTL_STATE_CLIMB, diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 7aec8922c2..1f15e2913f 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -92,7 +92,7 @@ Takeoff::on_active() // set loiter item so position controllers stop doing takeoff logic set_loiter_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); } @@ -149,7 +149,7 @@ Takeoff::set_takeoff_position() // convert mission item to current setpoint struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - mission_apply_limitation(&_mission_item); + mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.yaw_valid = true; diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 22baf42e38..e71299f19f 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -225,7 +225,7 @@ void Battery::determineWarning(bool connected) { if (connected) { - // propagate warning state only if the state is higher, otherwise remain in current waringin state + // propagate warning state only if the state is higher, otherwise remain in current warning state if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) { _warning = battery_status_s::BATTERY_WARNING_EMERGENCY;