Copter: add Mot and Rate dataflash logging

ArduCopter/ArduCopter.pde

@@ -1062,6 +1062,12 @@ static void ten_hz_logging_loop()
     if (should_log(MASK_LOG_ATTITUDE_MED)) {
         Log_Write_Attitude();
     }
+    if (should_log(MASK_LOG_RATE)) {
+        Log_Write_Rate();
+    }
+    if (should_log(MASK_LOG_MOT)) {
+        Log_Write_Mot();
+    }
     if (should_log(MASK_LOG_RCIN)) {
         DataFlash.Log_Write_RCIN();
     }

ArduCopter/Log.pde

@@ -360,6 +360,28 @@ static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
     DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd);
 }
 
+struct PACKED log_Rate {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float   control_roll;
+    float   roll;
+    float   roll_out;
+    float   control_pitch;
+    float   pitch;
+    float   pitch_out;
+    float   control_yaw;
+    float   yaw;
+    float   yaw_out;
+};
+
+struct PACKED log_Mot {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float   lift_max;
+    float   bat_volt;
+    float   bat_res;
+    float   th_limit;
+};
 
 // Write an attitude packet
 static void Log_Write_Attitude()
@@ -380,6 +402,40 @@ static void Log_Write_Attitude()
 #endif
 }
 
+// Write an rate packet
+static void Log_Write_Rate()
+{
+    const Vector3f &rate_targets = attitude_control.rate_bf_targets();
+    struct log_Rate pkt_rate = {
+        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
+        time_ms         : hal.scheduler->millis(),
+        control_roll    : (float)rate_targets.x,
+        roll            : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
+        roll_out        : (float)(motors.get_roll()),
+        control_pitch   : (float)rate_targets.y,
+        pitch           : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
+        pitch_out       : (float)(motors.get_pitch()),
+        control_yaw     : (float)rate_targets.z,
+        yaw             : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
+        yaw_out         : (float)(motors.get_yaw())
+    };
+    DataFlash.WriteBlock(&pkt_rate, sizeof(pkt_rate));
+}
+
+// Write an rate packet
+static void Log_Write_Mot()
+{
+    struct log_Mot pkt_mot = {
+        LOG_PACKET_HEADER_INIT(LOG_MOT_MSG),
+        time_ms         : hal.scheduler->millis(),
+        lift_max        : (float)(motors.get_lift_max()),
+        bat_volt        : (float)(motors.get_batt_voltage_filt()),
+        bat_res         : (float)(motors.get_batt_resistance()),
+        th_limit        : (float)(motors.get_throttle_limit())
+    };
+    DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
+}
+
 struct PACKED log_Startup {
     LOG_PACKET_HEADER;
 };
@@ -546,6 +602,10 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
       "PM",  "HHIhBHB",    "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
+    { LOG_RATE_MSG, sizeof(log_Rate),
+      "RATE", "Ifffffffff",  "TimeMS,RllDes,Rll,RllOut,PitDes,Pit,PitOut,YawDes,Yaw,YawOut" },
+    { LOG_MOT_MSG, sizeof(log_Mot),
+      "MOT", "Iffff",  "TimeMS,LiftMax,BatVolt,BatRes,ThLimit" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
       "STRT", "",            "" },
     { LOG_EVENT_MSG, sizeof(log_Event),         
@@ -620,6 +680,8 @@ static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) {}
 #endif
 static void Log_Write_Current() {}
 static void Log_Write_Attitude() {}
+static void Log_Write_Rate() {}
+static void Log_Write_Mot() {}
 static void Log_Write_Data(uint8_t id, int16_t value){}
 static void Log_Write_Data(uint8_t id, uint16_t value){}
 static void Log_Write_Data(uint8_t id, int32_t value){}

ArduCopter/config.h

@@ -754,7 +754,9 @@
     MASK_LOG_RCOUT | \
     MASK_LOG_OPTFLOW | \
     MASK_LOG_COMPASS | \
-    MASK_LOG_CAMERA
+    MASK_LOG_CAMERA | \
+    MASK_LOG_RATE | \
+    MASK_LOG_MOT
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/defines.h

@@ -236,6 +236,8 @@ enum FlipState {
 #define LOG_DATA_FLOAT_MSG              0x18
 #define LOG_AUTOTUNE_MSG                0x19
 #define LOG_AUTOTUNEDETAILS_MSG         0x1A
+#define LOG_RATE_MSG                    0x1D
+#define LOG_MOT_MSG                     0x1E
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)
@@ -254,6 +256,8 @@ enum FlipState {
 #define MASK_LOG_INAV                   (1<<14) // deprecated
 #define MASK_LOG_CAMERA                 (1<<15)
 #define MASK_LOG_WHEN_DISARMED          (1UL<<16)
+#define MASK_LOG_RATE                   (1UL<<17)
+#define MASK_LOG_MOT                    (1UL<<18)
 #define MASK_LOG_ANY                    0xFFFF
 
 // DATA - event logging