From 5a35d20b7ba7f13356050096024c416235ba7a1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 21:18:32 +0200 Subject: 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 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 84 insertions(+), 2 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..7000dd6c0 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) { -- cgit v1.2.3 From 67422c9896fb952ccf6db555aeb6f99224f8178b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:41:13 +0200 Subject: foh alt mode: never sink below previous wp alt --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7000dd6c0..b33b2049f 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 */ -- cgit v1.2.3