From f9b8afc0067d2e364a609364175f5b273b303d10 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 15 Jun 2017 11:22:11 +0200 Subject: [PATCH] Navigator: Use maximum flight altitude to limit missions This change limits all mission items to the maximum flight altitude. The mission will still be executed and flown, but the vehicle will never exceed the mission altitude. This ensures the vehicle can always reach the mission items. Wether or not the entire mission should be rejected if it falls outside of the fenced area is enforced in the mission feasibility checker function. --- .../land_detector/land_detector_params.c | 5 +++-- .../mc_pos_control/mc_pos_control_main.cpp | 14 +++++-------- src/modules/navigator/enginefailure.cpp | 2 ++ src/modules/navigator/follow_target.cpp | 1 + src/modules/navigator/land.cpp | 4 ++-- src/modules/navigator/loiter.cpp | 1 + src/modules/navigator/mission.cpp | 5 +++++ src/modules/navigator/mission_block.cpp | 21 +++++++++++++++++++ src/modules/navigator/mission_block.h | 6 ++++++ src/modules/navigator/navigator.h | 6 +----- src/modules/navigator/rcloss.cpp | 1 + src/modules/navigator/rtl.cpp | 2 ++ src/modules/navigator/rtl.h | 1 - src/modules/navigator/takeoff.cpp | 2 ++ 14 files changed, 52 insertions(+), 19 deletions(-) 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;