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

navigator: fix wrong altitude after takeoff

Browse Source 
This change fixes a wrong behaviour when a takeoff command is sent.

An example:
- MIS_TAKEOFF_ALT set to 10 meters
- Takeoff command with alt setpoint of 2 meters

Old behaviour:
1. Climb to 10 meters -> takeoff WP reached
2. Go to setpoint at 2 meters

New behaviour:
1. Climb to 10 meters and stay there but notify that altitude was
   overridden.
This commit is contained in:
Julian Oes
2016-10-23 16:25:15 +02:00
committed by Lorenz Meier
parent 032f4df3de
commit 82f27884df
3 changed files with 37 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+4 -12
View File
@@ -618,25 +618,17 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
}
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
{
item->nav_cmd = NAV_CMD_TAKEOFF;
/* use current position and use return altitude as clearance */
/* use current position */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
if (min_clearance > 0.0f) {
item->altitude += min_clearance;
/* we must takeoff to a point further above ground than the acceptance radius */
if (_navigator->get_acceptance_radius() > min_clearance) {
item->altitude += _navigator->get_acceptance_radius();
}
}
item->altitude = abs_altitude;
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;