fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/navigator/mission.cpp
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
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) */
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 */
@ -440,10 +450,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* calculate takeoff altitude */
float takeoff_alt = _mission_item.altitude;
if (_mission_item.altitude_is_relative) {
takeoff_alt += _navigator->get_home_position()->alt;
}
float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
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
* 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)) {
pos_sp_triplet->current.alt = _mission_item.altitude;
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} 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 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 a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;

2
src/modules/navigator/mission.h
View File

@ -132,6 +132,8 @@ private:
*/
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
* @return true if successful