diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 688db3ad39..1b0d759199 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -66,8 +66,6 @@ extern const AP_HAL::HAL& hal; #define AUTOTUNE_MIN_IMAX 2000 #define AUTOTUNE_MAX_IMAX 4000 -bool AP_AutoTune::logging_started = false; - // constructor AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, const AP_Vehicle::FixedWing &parms, @@ -290,42 +288,14 @@ void AP_AutoTune::save_gains(const ATGains &v) last_save = current; } -#define LOG_MSG_ATRP 211 - -struct PACKED log_ATRP { - LOG_PACKET_HEADER; - uint32_t timestamp; - uint8_t type; - uint8_t state; - int16_t servo; - float demanded; - float achieved; - float P; -}; - -static const struct LogStructure at_log_structures[] PROGMEM = { - { LOG_MSG_ATRP, sizeof(log_ATRP), - "ATRP", "IBBcfff", "TimeMS,Type,State,Servo,Demanded,Achieved,P" }, -}; - -void AP_AutoTune::write_log_headers(void) -{ - if (!logging_started) { - logging_started = true; - dataflash.AddLogFormats(at_log_structures, 1); - } -} - void AP_AutoTune::write_log(float servo, float demanded, float achieved) { if (!dataflash.logging_started()) { return; } - write_log_headers(); - struct log_ATRP pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_ATRP), + LOG_PACKET_HEADER_INIT(LOG_ATRP_MSG), timestamp : hal.scheduler->millis(), type : type, state : (uint8_t)state, diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 700f92db40..fd7b60ff14 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -23,6 +23,18 @@ public: AUTOTUNE_PITCH = 1 }; + struct PACKED log_ATRP { + LOG_PACKET_HEADER; + uint32_t timestamp; + uint8_t type; + uint8_t state; + int16_t servo; + float demanded; + float achieved; + float P; + }; + + // constructor AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash); @@ -54,9 +66,6 @@ private: // did we saturate surfaces? bool saturated_surfaces:1; - // have we sent log headers - static bool logging_started; - // values to restore if we leave autotune mode ATGains restore;