From 6f5b5c24f21362cf0f8a71d12a957118ac99cd08 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 30 Apr 2015 14:03:59 +0900 Subject: [PATCH] Copter: rename tkoff_ to takeoff_ --- ArduCopter/control_althold.pde | 6 +++--- ArduCopter/control_loiter.pde | 6 +++--- ArduCopter/control_poshold.pde | 6 +++--- ArduCopter/control_sport.pde | 6 +++--- ArduCopter/flight_mode.pde | 2 +- ArduCopter/takeoff.pde | 12 +++++------- 6 files changed, 18 insertions(+), 20 deletions(-) diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index f1f2fa2a9d..1606a2e04e 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -44,12 +44,12 @@ static void althold_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed()); + target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed()); // check for take-off if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { - tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off @@ -75,7 +75,7 @@ static void althold_run() // call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); - tkoff_increment_alt_target(G_Dt); + takeoff_increment_alt_target(G_Dt); pos_control.update_z_controller(); } } diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index a0bdf8265c..c94111ddfb 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -53,12 +53,12 @@ static void loiter_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed()); + target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed()); // check for take-off if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { - tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off @@ -99,7 +99,7 @@ static void loiter_run() // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); - tkoff_increment_alt_target(G_Dt); + takeoff_increment_alt_target(G_Dt); pos_control.update_z_controller(); } } diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index aa8112189b..37907a9d5d 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -171,12 +171,12 @@ static void poshold_run() // get pilot desired climb rate (for alt-hold mode and take-off) target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed()); + target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed()); // check for take-off if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { - tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off @@ -526,7 +526,7 @@ static void poshold_run() } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); - tkoff_increment_alt_target(G_Dt); + takeoff_increment_alt_target(G_Dt); pos_control.update_z_controller(); } } diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 9e7b3cfba1..66077cdfda 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -64,12 +64,12 @@ static void sport_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed()); + target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-takeoff_get_speed(), g.pilot_velocity_z_max-takeoff_get_speed()); // check for take-off if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) { if (!takeoff_state.running) { - tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } // indicate we are taking off @@ -96,7 +96,7 @@ static void sport_run() // call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); - tkoff_increment_alt_target(G_Dt); + takeoff_increment_alt_target(G_Dt); pos_control.update_z_controller(); } } diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 6d1e5a66d0..8a1e70bafe 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -129,7 +129,7 @@ static bool set_mode(uint8_t mode) // called at 100hz or more static void update_flight_mode() { - tkoff_timer_update(); + takeoff_timer_update(); #if AP_AHRS_NAVEKF_AVAILABLE // Update EKF speed limit - used to limit speed when we are using optical flow diff --git a/ArduCopter/takeoff.pde b/ArduCopter/takeoff.pde index 9e5c771664..427914f75f 100644 --- a/ArduCopter/takeoff.pde +++ b/ArduCopter/takeoff.pde @@ -29,16 +29,14 @@ static bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate) case ALT_HOLD: case SPORT: set_auto_armed(true); - tkoff_timer_start(pv_alt_above_origin(takeoff_alt_cm)-pos_control.get_pos_target().z); + takeoff_timer_start(pv_alt_above_origin(takeoff_alt_cm)-pos_control.get_pos_target().z); return true; } } return false; } - - -static void tkoff_timer_start(float alt) +static void takeoff_timer_start(float alt) { float speed = min(wp_nav.get_speed_up(), max(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); @@ -52,14 +50,14 @@ static void tkoff_timer_start(float alt) takeoff_state.time_ms = (alt/takeoff_state.speed) * 1.0e3f; } -static void tkoff_timer_update() +static void takeoff_timer_update() { if (millis()-takeoff_state.start_ms >= takeoff_state.time_ms) { takeoff_state.running = false; } } -static void tkoff_increment_alt_target(float dt) +static void takeoff_increment_alt_target(float dt) { if (takeoff_state.running) { Vector3f pos_target = pos_control.get_pos_target(); @@ -68,7 +66,7 @@ static void tkoff_increment_alt_target(float dt) } } -static float tkoff_get_speed() +static float takeoff_get_speed() { return takeoff_state.running?takeoff_state.speed:0.0f; }