From 5a35d20b7ba7f13356050096024c416235ba7a1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 21:18:32 +0200 Subject: [PATCH 1/3] FOH mode for altitude This introduces a parameter for the navigator. When enabled the navigator publishes a first order old (FOH) type altitude setpoint instead of the default zero order hold. For takeoff and landing the FOH mode is not active. The FOH altitude is calculated such that the sp reaches the altitude of the waypoint when the system is at a horizontal distance equal to the acceptance radius. Also the altitude setpoint will only converge towards the waypoint altitude but never diverge. --- src/modules/navigator/mission.cpp | 86 +++++++++++++++++++++++++- src/modules/navigator/mission.h | 25 +++++++- src/modules/navigator/mission_params.c | 13 ++++ 3 files changed, 121 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3eda..7000dd6c0c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -36,12 +36,14 @@ * Helper class to access missions * * @author Julian Oes + * @author Thomas Gubler */ #include #include #include #include +#include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include #include @@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), _param_dist_1wp(this, "DIST_1WP"), + _param_altmode(this, "ALTMODE"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false) + _dist_1wp_ok(false), + _missionFeasiblityChecker(), + _min_current_sp_distance_xy(FLT_MAX), + _mission_item_previous_alt(NAN), + _distance_current_previous(0.0f) { /* load initial params */ updateParams(); @@ -144,6 +152,8 @@ Mission::on_active() advance_mission(); set_mission_items(); + } else if (_mission_type != MISSION_TYPE_NONE &&_param_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) { @@ -202,7 +212,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); @@ -313,11 +323,19 @@ Mission::set_mission_items() /* make sure param is up to date */ updateParams(); + /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ + altitude_sp_foh_reset(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ + if (pos_sp_triplet->previous.valid) { + _mission_item_previous_alt = _mission_item.altitude; + } + /* get home distance state */ bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ @@ -446,9 +464,73 @@ 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(); } +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 */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_mission_item_previous_alt)) { + return; + } + + /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ + if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + 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 */ + _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + + /* 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 < _mission_item.acceptance_radius) { + pos_sp_triplet->current.alt = _mission_item.altitude; + } 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 = (_mission_item.altitude - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float a = _mission_item_previous_alt - grad * _distance_current_previous; + pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; + + } + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Mission::altitude_sp_foh_reset() +{ + _min_current_sp_distance_xy = FLT_MAX; +} + bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..f7d12da702 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -36,6 +36,7 @@ * Navigator mode to access missions * * @author Julian Oes + * @author Thomas Gubler */ #ifndef NAVIGATOR_MISSION_H @@ -75,6 +76,11 @@ public: virtual void on_active(); + enum mission_altitude_mode { + MISSION_ALTMODE_ZOH = 0, + MISSION_ALTMODE_FOH =1 + }; + private: /** * Update onboard mission topic @@ -102,6 +108,16 @@ private: */ void set_mission_items(); + /** + * Updates the altitude sp to follow a foh + */ + void altitude_sp_foh_update(); + + /** + * Resets the altitude sp foh logic + */ + void altitude_sp_foh_reset(); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful @@ -136,6 +152,7 @@ private: control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; + control::BlockParamInt _param_altmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -157,7 +174,13 @@ private: bool _inited; bool _dist_1wp_ok; - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + + float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ + float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, + can be replaced by a full copy of the previous mission item if needed*/ + float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, + only use if current and previous are valid */ }; #endif diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d15e1e771b..04c01fe51c 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @group Mission */ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); + +/** + * 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 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ALTMODE, 0); From 9cf5e6f7b268acd732a14e577752dc32aeaf9430 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 22:08:03 +0200 Subject: [PATCH 2/3] whitespace --- src/modules/navigator/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f7d12da702..a91c899687 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,7 +78,7 @@ public: enum mission_altitude_mode { MISSION_ALTMODE_ZOH = 0, - MISSION_ALTMODE_FOH =1 + MISSION_ALTMODE_FOH = 1 }; private: From 67422c9896fb952ccf6db555aeb6f99224f8178b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:41:13 +0200 Subject: [PATCH 3/3] foh alt mode: never sink below previous wp alt --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7000dd6c0c..b33b2049fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -502,8 +502,10 @@ Mission::altitude_sp_foh_update() 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 */ - _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + /* 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 */