Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: fix takeoff to terrain alt

This commit is contained in:
Randy Mackay 2022-03-11 13:09:16 +09:00
parent 759e2b1b55
commit 553ad877f6
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.h
View File

@ -213,7 +213,7 @@ protected:
// auto takeoff variables
static float auto_takeoff_start_alt_cm; // start altitude expressed as cm above ekf origin
static float auto_takeoff_complete_alt_cm; // completion altitude expressed as cm above ekf origin
static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain
static bool auto_takeoff_complete; // true when takeoff is complete
static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm

2
ArduCopter/takeoff.cpp
View File

@ -179,7 +179,7 @@ void Mode::auto_takeoff_run()
}
// handle takeoff completion
bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm - auto_takeoff_start_alt_cm) * 0.90);
bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90);
bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1;
auto_takeoff_complete = reached_altitude && reached_climb_rate;