Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: log quiet-nan for rangefinder-desired-alt when not using it

Browse Source
This commit is contained in:
Peter Barker 2018-06-05 18:21:09 +10:00 committed by Randy Mackay
parent eac5d13f1c
commit ab68e4fe8b
4 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.cpp
View File

@ -170,6 +170,8 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
uint32_t now = millis(); uint32_t now = millis();
target_rangefinder_alt_used = true;
// reset target altitude if this controller has just been engaged // reset target altitude if this controller has just been engaged
if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) { if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt; target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;

1
ArduCopter/Copter.h
View File

@ -463,6 +463,7 @@ private:
// The cm/s we are moving up or down based on filtered data - Positive = UP // The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate; int16_t climb_rate;
float target_rangefinder_alt; // desired altitude in cm above the ground 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 int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s float baro_climbrate; // barometer climbrate in cm/s
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests

12
ArduCopter/Log.cpp
View File

@ -102,7 +102,7 @@ struct PACKED log_Control_Tuning {
float desired_alt; float desired_alt;
float inav_alt; float inav_alt;
int32_t baro_alt; int32_t baro_alt;
int16_t desired_rangefinder_alt; float desired_rangefinder_alt;
int16_t rangefinder_alt; int16_t rangefinder_alt;
float terr_alt; float terr_alt;
int16_t target_climb_rate; int16_t target_climb_rate;
@ -120,6 +120,12 @@ void Copter::Log_Write_Control_Tuning()
} }
#endif #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 = { struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
@ -130,7 +136,7 @@ void Copter::Log_Write_Control_Tuning()
desired_alt : pos_control->get_alt_target() / 100.0f, desired_alt : pos_control->get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt, baro_alt : baro_alt,
desired_rangefinder_alt : (int16_t)target_rangefinder_alt, desired_rangefinder_alt : _target_rangefinder_alt,
rangefinder_alt : rangefinder_state.alt_cm, rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt, terr_alt : terr_alt,
target_climb_rate : (int16_t)pos_control->get_vel_target_z(), 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" }, "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" },
#endif #endif
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), { 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), { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" }, "MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_EVENT_MSG, sizeof(log_Event), { LOG_EVENT_MSG, sizeof(log_Event),

2
ArduCopter/mode.cpp
View File

@ -247,6 +247,8 @@ void Copter::update_flight_mode()
// Update EKF speed limit - used to limit speed when we are using optical flow // Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
target_rangefinder_alt_used = false;
flightmode->run(); flightmode->run();
} }