diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index c579e97ccf..893e449db2 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -170,6 +170,8 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current uint32_t now = millis(); + target_rangefinder_alt_used = true; + // reset target altitude if this controller has just been engaged if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) { target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f7160a885e..18b1c18e0e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -463,6 +463,7 @@ private: // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; float target_rangefinder_alt; // desired altitude in cm above the ground + bool target_rangefinder_alt_used; // true if mode is using target_rangefinder_alt int32_t baro_alt; // barometer altitude in cm above home float baro_climbrate; // barometer climbrate in cm/s LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index cd0f3d4303..394cac7738 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -102,7 +102,7 @@ struct PACKED log_Control_Tuning { float desired_alt; float inav_alt; int32_t baro_alt; - int16_t desired_rangefinder_alt; + float desired_rangefinder_alt; int16_t rangefinder_alt; float terr_alt; int16_t target_climb_rate; @@ -120,6 +120,12 @@ void Copter::Log_Write_Control_Tuning() } #endif + float _target_rangefinder_alt; + if (target_rangefinder_alt_used) { + _target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m + } else { + _target_rangefinder_alt = DataFlash.quiet_nan(); + } struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), @@ -130,7 +136,7 @@ void Copter::Log_Write_Control_Tuning() desired_alt : pos_control->get_alt_target() / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : baro_alt, - desired_rangefinder_alt : (int16_t)target_rangefinder_alt, + desired_rangefinder_alt : _target_rangefinder_alt, rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control->get_vel_target_z(), @@ -492,7 +498,7 @@ const struct LogStructure Copter::log_structure[] = { "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" }, #endif { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qffffffeccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" }, + "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), "MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" }, { LOG_EVENT_MSG, sizeof(log_Event), diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a9ca196e3c..b7a3010f3a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -247,6 +247,8 @@ void Copter::update_flight_mode() // Update EKF speed limit - used to limit speed when we are using optical flow ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + target_rangefinder_alt_used = false; + flightmode->run(); }