From 5ac13c0355fcefff47f30fd3fc96d3f89aa56bc6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 7 May 2016 16:55:40 +0900 Subject: [PATCH] Copter: move rangefinder variables into structure moved in rangefinder_alt, rangefinder_alt_health and rangefinder_enabled --- ArduCopter/Attitude.cpp | 6 +++--- ArduCopter/Copter.cpp | 3 --- ArduCopter/Copter.h | 8 +++++--- ArduCopter/Log.cpp | 2 +- ArduCopter/control_land.cpp | 4 ++-- ArduCopter/heli.cpp | 2 +- ArduCopter/precision_landing.cpp | 2 +- ArduCopter/sensors.cpp | 12 ++++++------ ArduCopter/switches.cpp | 4 ++-- 9 files changed, 21 insertions(+), 22 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 70f21361a3..d9e6115dd2 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -255,7 +255,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current // reset target altitude if this controller has just been engaged if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) { - target_rangefinder_alt = rangefinder_alt + current_alt_target - current_alt; + target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt; } last_call_ms = now; @@ -266,10 +266,10 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current // do not let target altitude get too far from current altitude above ground // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition - target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_alt-pos_control.get_leash_down_z(),rangefinder_alt+pos_control.get_leash_up_z()); + target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z()); // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) - distance_error = (target_rangefinder_alt - rangefinder_alt) - (current_alt_target - current_alt); + distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt); velocity_correction = distance_error * g.rangefinder_gain; velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX); diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index fa0435e3e2..625f28e6f2 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -25,7 +25,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Copter::Copter(void) : DataFlash{FIRMWARE_STRING}, flight_modes(&g.flight_mode1), - rangefinder_enabled(true), mission(ahrs, FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &), @@ -60,8 +59,6 @@ Copter::Copter(void) : frsky_telemetry(ahrs, battery), #endif climb_rate(0), - rangefinder_alt(0), - rangefinder_alt_health(0), target_rangefinder_alt(0.0f), baro_alt(0), baro_climbrate(0.0f), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ff58509f02..4b34fa3557 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -171,7 +171,11 @@ private: #if RANGEFINDER_ENABLED == ENABLED RangeFinder rangefinder {serial_manager}; - bool rangefinder_enabled; // enable user switch for rangefinder + struct { + bool enabled:1; + bool alt_healthy:1; // true if we can trust the altitude from the rangefinder + int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder + } rangefinder_state = { true, false, 0 }; #endif AP_RPM rpm_sensor; @@ -403,8 +407,6 @@ private: // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; - int16_t rangefinder_alt; // altitude as reported by the rangefinder in cm - uint8_t rangefinder_alt_health; // true if we can trust the altitude from the rangefinder float target_rangefinder_alt; // desired altitude in cm above the ground int32_t baro_alt; // barometer altitude in cm above home float baro_climbrate; // barometer climbrate in cm/s diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 448ddfab98..e23d3c12e4 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -331,7 +331,7 @@ void Copter::Log_Write_Control_Tuning() inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : baro_alt, desired_rangefinder_alt : (int16_t)target_rangefinder_alt, - rangefinder_alt : rangefinder_alt, + rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, desired_climb_rate : (int16_t)pos_control.get_vel_target_z(), climb_rate : climb_rate diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 75f556c884..1425f693fd 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -234,7 +234,7 @@ void Copter::land_nogps_run() float Copter::get_land_descent_speed() { #if RANGEFINDER_ENABLED == ENABLED - bool rangefinder_ok = rangefinder_enabled && (rangefinder.status() == RangeFinder::RangeFinder_Good); + bool rangefinder_ok = rangefinder_state.enabled && (rangefinder.status() == RangeFinder::RangeFinder_Good); #else bool rangefinder_ok = false; #endif @@ -252,7 +252,7 @@ float Copter::get_land_descent_speed() } // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent - if ((target_alt_cm >= LAND_START_ALT) && !(rangefinder_ok && rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + if ((target_alt_cm >= LAND_START_ALT) && !(rangefinder_ok && rangefinder_state.alt_healthy)) { if (g.land_speed_high > 0) { // user has asked for a different landing speed than normal descent rate return -abs(g.land_speed_high); diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 607c64e9ea..309076c95c 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -44,7 +44,7 @@ void Copter::check_dynamic_flight(void) moving = (motors.get_throttle() > 0.8f || ahrs.pitch_sensor < -1500); } - if (!moving && rangefinder_enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) { + if (!moving && rangefinder_state.enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) { // when we are more than 2m from the ground with good // rangefinder lock consider it to be dynamic flight moving = (rangefinder.distance_cm() > 200); diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 5fb594e338..4b4592ad05 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -19,7 +19,7 @@ void Copter::update_precland() // use range finder altitude if it is valid if (rangefinder_alt_ok()) { - final_alt = rangefinder_alt; + final_alt = rangefinder_state.alt_cm; } copter.precland.update(final_alt); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index a69075866b..f513f586d8 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -41,11 +41,11 @@ void Copter::read_rangefinder(void) // exit immediately if rangefinder is disabled if (rangefinder.status() != RangeFinder::RangeFinder_Good) { - rangefinder_alt_health = 0; + rangefinder_state.alt_healthy = false; return; } - rangefinder_alt_health = rangefinder.range_valid_count(); + rangefinder_state.alt_healthy = (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX); int16_t temp_alt = rangefinder.distance_cm(); @@ -54,17 +54,17 @@ void Copter::read_rangefinder(void) temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); #endif - rangefinder_alt = temp_alt; + rangefinder_state.alt_cm = temp_alt; #else - rangefinder_alt_health = 0; - rangefinder_alt = 0; + rangefinder_state.alt_healthy = false; + rangefinder_state.alt_cm = 0; #endif } // return true if rangefinder_alt can be used bool Copter::rangefinder_alt_ok() { - return (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)); + return (rangefinder_state.enabled && rangefinder_state.alt_healthy); } /* diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 989fb362f1..f00e7078f5 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -349,9 +349,9 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { - rangefinder_enabled = true; + rangefinder_state.enabled = true; }else{ - rangefinder_enabled = false; + rangefinder_state.enabled = false; } #endif break;