Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: CTUN logging fix for SAlt 

sonar altitude was not being logged in modes that don't use surface tracking including Auto
SAlt scaling was also incorrect
This commit is contained in:
Randy Mackay 2019-11-11 12:55:43 +09:00
parent d4438f0a1c
commit 69e8158a9d
3 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Copter.h
View File

@ -292,7 +292,8 @@ private:
void set_target_alt_cm(float target_alt_cm); void set_target_alt_cm(float target_alt_cm);
// get target and actual distances (in m) for logging purposes // get target and actual distances (in m) for logging purposes
bool get_dist_for_logging(float &target_dist, float &actual_dist) const; bool get_target_dist_for_logging(float &target_dist) const;
float get_dist_for_logging() const;
void invalidate_for_logging() { valid_for_logging = false; } void invalidate_for_logging() { valid_for_logging = false; }
// surface tracking surface // surface tracking surface

9
ArduCopter/Log.cpp
View File

@ -41,10 +41,9 @@ void Copter::Log_Write_Control_Tuning()
} }
// get surface tracking alts // get surface tracking alts
float desired_rangefinder_alt, rangefinder_alt; float desired_rangefinder_alt;
if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) { if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
desired_rangefinder_alt = AP::logger().quiet_nan(); desired_rangefinder_alt = AP::logger().quiet_nan();
rangefinder_alt = AP::logger().quiet_nan();;
} }
struct log_Control_Tuning pkt = { struct log_Control_Tuning pkt = {
@ -58,7 +57,7 @@ void Copter::Log_Write_Control_Tuning()
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 : desired_rangefinder_alt, desired_rangefinder_alt : desired_rangefinder_alt,
rangefinder_alt : rangefinder_alt, rangefinder_alt : surface_tracking.get_dist_for_logging(),
terr_alt : terr_alt, terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms, target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t climb_rate : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
@ -463,7 +462,7 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" }, "PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B0B0BB-" }, "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B000BB-" },
{ 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_DATA_INT16_MSG, sizeof(log_Data_Int16t), { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),

9
ArduCopter/surface_tracking.cpp
View File

@ -86,16 +86,21 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
last_update_ms = AP_HAL::millis(); last_update_ms = AP_HAL::millis();
} }
bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
{ {
if (!valid_for_logging || (surface == Surface::NONE)) { if (!valid_for_logging || (surface == Surface::NONE)) {
return false; return false;
} }
target_dist = target_dist_cm * 0.01f; target_dist = target_dist_cm * 0.01f;
actual_dist = ((surface == Surface::GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
return true; return true;
} }
float Copter::SurfaceTracking::get_dist_for_logging() const
{
return ((surface == Surface::CEILING) ? copter.rangefinder_up_state.alt_cm : copter.rangefinder_state.alt_cm) * 0.01f;
}
// set direction // set direction
void Copter::SurfaceTracking::set_surface(Surface new_surface) void Copter::SurfaceTracking::set_surface(Surface new_surface)
{ {