APM_Control: added dataflash logging to autotune

libraries/APM_Control/AP_AutoTune.cpp

@@ -64,12 +64,15 @@ extern const AP_HAL::HAL& hal;
 #define AUTOTUNE_MIN_P 0.3f
 
 // constructor
-AP_AutoTune::AP_AutoTune(ATGains &_gains) :
-current(_gains) {}
+AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, DataFlash_Class &_dataflash) :
+    current(_gains),
+    type(_type),
+    dataflash(_dataflash)
+{}
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #include <stdio.h>
- # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+# define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
 #else
  # define Debug(fmt, args ...)
 #endif
@@ -165,6 +168,9 @@ void AP_AutoTune::update(float desired_rate, float achieved_rate, float servo_ou
         state_enter_ms = now;
         saturated_surfaces = false;
     }
+    if (state != DEMAND_UNSATURATED) {
+        write_log(servo_out, desired_rate, achieved_rate);
+    }
 }
 
 /*
@@ -241,3 +247,50 @@ void AP_AutoTune::save_gains(const ATGains &v)
     current.rmax.set_and_save_ifchanged(v.rmax);
     current.imax.set_and_save_ifchanged(v.imax);
 }
+
+#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", "IBBhfff",  "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(int16_t servo, float demanded, float achieved)
+{
+    if (!dataflash.logging_started()) {
+        return;
+    }
+
+    write_log_headers();
+
+    struct log_ATRP pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_MSG_ATRP),
+        timestamp  : hal.scheduler->millis(),
+        type       : type,
+    	state      : (uint8_t)state,
+        servo      : servo,
+        demanded   : demanded,
+        achieved   : achieved,
+        P          : current.P.get()
+    };
+    dataflash.WriteBlock(&pkt, sizeof(pkt));    
+}

libraries/APM_Control/AP_AutoTune.h

@@ -5,6 +5,7 @@
 
 #include <AP_HAL.h>
 #include <AP_Param.h>
+#include <DataFlash.h>
 
 class AP_AutoTune {
 public:
@@ -17,8 +18,13 @@ public:
         AP_Int16 imax;
     };
 
+    enum ATType {
+        AUTOTUNE_ROLL  = 0,
+        AUTOTUNE_PITCH = 1
+    };
+
     // constructor
-    AP_AutoTune(ATGains &_gains);
+    AP_AutoTune(ATGains &_gains, ATType type, DataFlash_Class &_dataflash);
 
     // called when autotune mode is entered
     void start(void);
@@ -38,9 +44,17 @@ private:
     // the current gains
     ATGains &current;
 
+    // what type of autotune is this
+    ATType type;
+
+    DataFlash_Class &dataflash;
+
     // did we saturate surfaces?
     bool saturated_surfaces:1;
 
+    // have we sent log headers
+    bool logging_started:1;
+
     // values to restore if we leave autotune mode
     ATGains restore; 
 
@@ -63,6 +77,9 @@ private:
     void check_save(void);
     void check_state_exit(uint32_t state_time_ms);
     void save_gains(const ATGains &v);
+
+    void write_log_headers(void);
+    void write_log(int16_t servo, float demanded, float achieved);
 };
 
 #endif // __AP_AUTOTUNE_H__

libraries/APM_Control/AP_PitchController.h

@@ -7,13 +7,14 @@
 #include <AP_Common.h>
 #include <AP_Vehicle.h>
 #include <AP_AutoTune.h>
-#include <math.h>
+#include <DataFlash.h>
+#include <AP_Math.h>
 
 class AP_PitchController {
 public:
-	AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
+	AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) :
 		aparm(parms),
-        autotune(gains),
+        autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, _dataflash),
         _ahrs(ahrs)
     { 
 		AP_Param::setup_object_defaults(this, var_info);

libraries/APM_Control/AP_RollController.h

@@ -7,13 +7,14 @@
 #include <AP_Common.h>
 #include <AP_Vehicle.h>
 #include <AP_AutoTune.h>
-#include <math.h>
+#include <DataFlash.h>
+#include <AP_Math.h>
 
 class AP_RollController {
 public:
-	AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
+	AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms, DataFlash_Class &_dataflash) :
 		aparm(parms),
-        autotune(gains),
+        autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, _dataflash),
         _ahrs(ahrs)
     { 
 		AP_Param::setup_object_defaults(this, var_info);