diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 0ebffb9023..5786db9da6 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -206,12 +206,13 @@ 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 * * @unit m - * @min 1.5 + * @min -1 * @max 10000 * @decimal 2 * @group Land Detector * */ -PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, 10000.0f); +PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); 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 5d71dd407c..f5bb9a8db0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -920,12 +920,9 @@ void MulticopterPositionControl::limit_altitude() { if (_vehicle_land_detected.alt_max > 0.0f) { - - float altitude_above_home = -_pos(2) + _home_pos.z; - - /* in altitude control, lim_pos_sp(2)it setpoint */ - if (_run_alt_control && altitude_above_home > _vehicle_land_detected.alt_max) { - _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; + /* 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; } @@ -939,8 +936,8 @@ MulticopterPositionControl::limit_altitude() 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) { - _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; + if (pos_z_next <= -_vehicle_land_detected.alt_max) { + _pos_sp(2) = -_vehicle_land_detected.alt_max; _run_alt_control = true; return; } @@ -948,7 +945,6 @@ MulticopterPositionControl::limit_altitude() } } - bool MulticopterPositionControl::in_auto_takeoff() { diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index 5ff5385671..9f71bc6527 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -126,7 +126,9 @@ 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_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 116144c2aa..182aaa1fbd 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -361,6 +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_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 d440c84406..6d2908d616 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -81,12 +81,12 @@ 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_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 facea22d63..eaf9a71f59 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -129,6 +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_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 4e1f0187f3..3e7492927a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -508,6 +508,7 @@ Mission::set_mission_items() /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; + mission_apply_limitation(&_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; @@ -764,6 +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_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); } @@ -806,6 +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_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); /* issue command if ready (will do nothing for position mission items) */ @@ -832,6 +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_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); } else { @@ -1183,6 +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_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 54bbe2580e..55c565f4dd 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -749,3 +749,24 @@ MissionBlock::set_idle_item(struct mission_item_s *item) item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } + +void +MissionBlock::mission_apply_limitation(struct mission_item_s *item) +{ + + /* 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; + + 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; + + } + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 111ae9d9a3..893b9ab2c2 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -120,6 +120,12 @@ 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); + void issue_command(const mission_item_s &item); float get_time_inside(const struct mission_item_s &item); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d5b764e187..7fa4b6047b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -36,6 +36,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * @author Dennis Mannhart */ #ifndef NAVIGATOR_H @@ -323,11 +324,6 @@ private: */ void task_main(); - /** - * Translate mission item to a position setpoint. - */ - void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - /** * Publish a new position setpoint triplet for position controllers */ diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index fcc21b992a..54fe45c1ce 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -140,6 +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_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 c1d2566ce8..3b135a0f37 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -84,6 +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_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; @@ -291,6 +292,7 @@ RTL::set_rtl_item() } /* convert mission item to current position setpoint and make it valid */ + 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 94fa1d530e..2ed63d0d80 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -79,7 +79,6 @@ 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 d662fb9c7c..7aec8922c2 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -92,6 +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_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); } @@ -148,6 +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_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.yaw_valid = true;