fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Make logic using previous and current altitudes consistent

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-11 21:05:38 +02:00
parent 891829d3a7
commit 7540aa6b87
2 changed files with 16 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+14 -7
View File
@@ -271,6 +271,16 @@ Mission::advance_mission()
} }
} }
int
Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
{
if (_mission_item.altitude_is_relative) {
return _mission_item.altitude + _navigator->get_home_position()->alt;
} else {
return _mission_item.altitude;
}
}
bool bool
Mission::check_dist_1wp() Mission::check_dist_1wp()
{ {
@@ -354,7 +364,7 @@ Mission::set_mission_items()
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) { if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = _mission_item.altitude; _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
} }
/* get home distance state */ /* get home distance state */
@@ -440,10 +450,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* calculate takeoff altitude */ /* calculate takeoff altitude */
float takeoff_alt = _mission_item.altitude; float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
if (_mission_item.altitude_is_relative) {
takeoff_alt += _navigator->get_home_position()->alt;
}
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) { if (_navigator->get_vstatus()->condition_landed) {
@@ -609,14 +616,14 @@ Mission::altitude_sp_foh_update()
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt /* 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 */ * 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 < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
pos_sp_triplet->current.alt = _mission_item.altitude; pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} else { } else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp /* 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 * 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 * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint * radius around the current waypoint
**/ **/
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
float a = _mission_item_previous_alt - grad * _distance_current_previous; float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
src/modules/navigator/mission.h
+2
View File
@@ -132,6 +132,8 @@ private:
*/ */
void altitude_sp_foh_reset(); void altitude_sp_foh_reset();
int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
/** /**
* Read current or next mission item from the dataman and watch out for DO_JUMPS * Read current or next mission item from the dataman and watch out for DO_JUMPS
* @return true if successful * @return true if successful