From 759e2b1b55a8c6122b7346e71825c0e10b125e2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2022 11:48:56 +0900 Subject: [PATCH] Copter: rename auto_take_off_xx to auto_takeoff_xx --- ArduCopter/mode.h | 4 ++-- ArduCopter/takeoff.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index bf3e302a0b..d16cf93fdf 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -212,8 +212,8 @@ protected: static float auto_takeoff_no_nav_alt_cm; // auto takeoff variables - static float auto_take_off_start_alt_cm; // start altitude expressed as cm above ekf origin - static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin + 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 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 diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 7dfd4e2778..b7df4ea755 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -4,8 +4,8 @@ Mode::_TakeOff Mode::takeoff; bool Mode::auto_takeoff_no_nav_active = false; float Mode::auto_takeoff_no_nav_alt_cm = 0; -float Mode::auto_take_off_start_alt_cm = 0; -float Mode::auto_take_off_complete_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; Vector3p Mode::auto_takeoff_complete_pos; @@ -159,7 +159,7 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); // command the aircraft to the take off altitude - float pos_z = auto_take_off_complete_alt_cm + terr_offset; + float pos_z = auto_takeoff_complete_alt_cm + terr_offset; float vel_z = 0.0; copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0); @@ -179,7 +179,7 @@ void Mode::auto_takeoff_run() } // handle takeoff completion - bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_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 - 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; @@ -192,13 +192,13 @@ void Mode::auto_takeoff_run() void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt) { - auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm(); - auto_take_off_complete_alt_cm = complete_alt_cm; + auto_takeoff_start_alt_cm = inertial_nav.get_position_z_up_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_take_off_start_alt_cm + g2.wp_navalt_min * 100; + auto_takeoff_no_nav_alt_cm = auto_takeoff_start_alt_cm + g2.wp_navalt_min * 100; auto_takeoff_no_nav_active = true; } else { auto_takeoff_no_nav_active = false;