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

Copter: Fix Auto Takeoff when complete_alt_cm is current altitude

This commit is contained in:
Leonard Hall 2022-12-29 16:05:26 +10:30 committed by Randy Mackay
parent 0af7cd59e1
commit a234b6bda8
2 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -221,7 +221,6 @@ protected:
static float auto_takeoff_no_nav_alt_cm;
// 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 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

15
ArduCopter/takeoff.cpp
View File

@ -4,7 +4,6 @@ Mode::_TakeOff Mode::takeoff;
bool Mode::auto_takeoff_no_nav_active = false;
float Mode::auto_takeoff_no_nav_alt_cm = 0;
float Mode::auto_takeoff_start_alt_cm = 0;
float Mode::auto_takeoff_complete_alt_cm = 0;
bool Mode::auto_takeoff_terrain_alt = false;
bool Mode::auto_takeoff_complete = false;
@ -220,9 +219,13 @@ void Mode::auto_takeoff_run()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
// handle takeoff completion
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;
// takeoff complete when we are less than 1% of the stopping distance from the target altitude
// and 10% our maximum climb rate
const float vel_threshold_fraction = 0.1;
// stopping distance from vel_threshold_fraction * max velocity
const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance;
bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction;
auto_takeoff_complete = reached_altitude && reached_climb_rate;
// calculate completion for location in case it is needed for a smooth transition to wp_nav
@ -234,13 +237,13 @@ void Mode::auto_takeoff_run()
void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
{
auto_takeoff_start_alt_cm = inertial_nav.get_position_z_up_cm();
// auto_takeoff_complete_alt_cm is a problem if equal to auto_takeoff_start_alt_cm
auto_takeoff_complete_alt_cm = complete_alt_cm;
auto_takeoff_terrain_alt = terrain_alt;
auto_takeoff_complete = false;
if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
// we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
auto_takeoff_no_nav_alt_cm = auto_takeoff_start_alt_cm + g2.wp_navalt_min * 100;
auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
auto_takeoff_no_nav_active = true;
} else {
auto_takeoff_no_nav_active = false;