Tracker: add dataflash logging

AntennaTracker/AntennaTracker.cpp

@@ -40,6 +40,8 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
     SCHED_TASK(gcs_data_stream_send,   50,   3000),
     SCHED_TASK(compass_accumulate,     50,   1500),
     SCHED_TASK(barometer_accumulate,   50,    900),
+    SCHED_TASK(ten_hz_logging_loop,    10,    300),
+    SCHED_TASK(dataflash_periodic,     50,    300),
     SCHED_TASK(update_notify,          50,    100),
     SCHED_TASK(check_usb_mux,          10,    300),
     SCHED_TASK(gcs_retry_deferred,     50,   1000),
@@ -83,6 +85,11 @@ void Tracker::loop()
     scheduler.run(19900UL);
 }
 
+void Tracker::dataflash_periodic(void)
+{
+    DataFlash.periodic_tasks();
+}
+
 void Tracker::one_second_loop()
 {
     // send a heartbeat
@@ -107,6 +114,23 @@ void Tracker::one_second_loop()
     }
 }
 
+void Tracker::ten_hz_logging_loop()
+{
+    if (should_log(MASK_LOG_IMU)) {
+        DataFlash.Log_Write_IMU(ins);
+        DataFlash.Log_Write_IMUDT(ins);
+    }
+    if (should_log(MASK_LOG_ATTITUDE)) {
+        Log_Write_Attitude();
+    }
+    if (should_log(MASK_LOG_RCIN)) {
+        DataFlash.Log_Write_RCIN();
+    }
+    if (should_log(MASK_LOG_RCOUT)) {
+        DataFlash.Log_Write_RCOUT();
+    }
+}
+
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 Tracker::Tracker(void)

AntennaTracker/GCS_Mavlink.cpp

@@ -5,9 +5,6 @@
 // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
 #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)
 
-// use this to prevent recursion during sensor init
-static bool in_mavlink_delay;
-
 /*
  *  !!NOTE!!
  *
@@ -429,7 +426,7 @@ GCS_MAVLINK::data_stream_send(void)
         }
     }
 
-    if (in_mavlink_delay) {
+    if (tracker.in_mavlink_delay) {
         // don't send any other stream types while in the delay callback
         return;
     }
@@ -886,6 +883,26 @@ mission_failed:
         break;
     }
 
+    case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+    case MAVLINK_MSG_ID_LOG_ERASE:
+        tracker.in_log_download = true;
+        /* no break */
+    case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+        if (!tracker.in_mavlink_delay) {
+            handle_log_message(msg, tracker.DataFlash);
+        }
+        break;
+    case MAVLINK_MSG_ID_LOG_REQUEST_END:
+        tracker.in_log_download = false;
+        if (!tracker.in_mavlink_delay) {
+            handle_log_message(msg, tracker.DataFlash);
+        }
+        break;
+
+    case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
+        tracker.DataFlash.remote_log_block_status_msg(chan, msg);
+        break;
+
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
         handle_serial_control(msg, tracker.gps);
         break;
@@ -913,7 +930,7 @@ void Tracker::mavlink_delay_cb()
     static uint32_t last_1hz, last_50hz, last_5s;
     if (!gcs[0].initialised) return;
 
-    in_mavlink_delay = true;
+    tracker.in_mavlink_delay = true;
 
     uint32_t tnow = AP_HAL::millis();
     if (tnow - last_1hz > 1000) {
@@ -931,7 +948,7 @@ void Tracker::mavlink_delay_cb()
         last_5s = tnow;
         gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");
     }
-    in_mavlink_delay = false;
+    tracker.in_mavlink_delay = false;
 }
 
 /*

AntennaTracker/Log.cpp

@@ -0,0 +1,83 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Tracker.h"
+
+#if LOGGING_ENABLED == ENABLED
+
+// Code to Write and Read packets from DataFlash log memory
+
+// Write an attitude packet
+void Tracker::Log_Write_Attitude()
+{
+    Vector3f targets;
+    targets.y = nav_status.pitch;
+    targets.z = wrap_360_cd_float(nav_status.bearing);
+    DataFlash.Log_Write_Attitude(ahrs, targets);
+
+    DataFlash.Log_Write_EKF(ahrs,false);
+    DataFlash.Log_Write_AHRS2(ahrs);
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    sitl.Log_Write_SIMSTATE(&DataFlash);
+#endif
+    DataFlash.Log_Write_POS(ahrs);
+}
+
+void Tracker::Log_Write_Baro(void)
+{
+    DataFlash.Log_Write_Baro(barometer);
+}
+
+const struct LogStructure Tracker::log_structure[] = {
+    LOG_COMMON_STRUCTURES,
+};
+
+void Tracker::Log_Write_Vehicle_Startup_Messages()
+{
+    // only 200(?) bytes are guaranteed by DataFlash
+    DataFlash.Log_Write_Mode(control_mode);
+}
+
+// start a new log
+void Tracker::start_logging()
+{
+    if (g.log_bitmask != 0) {
+        if (!logging_started) {
+            logging_started = true;
+            DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
+            DataFlash.StartNewLog();
+        }
+        // enable writes
+        DataFlash.EnableWrites(true);
+    }
+}
+
+void Tracker::log_init(void)
+{
+    DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
+    if (!DataFlash.CardInserted()) {
+        gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted");
+        g.log_bitmask.set(0);
+    } else if (DataFlash.NeedPrep()) {
+        gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
+        DataFlash.Prep();
+        gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
+        for (uint8_t i=0; i<num_gcs; i++) {
+            gcs[i].reset_cli_timeout();
+        }
+    }
+
+    if (g.log_bitmask != 0) {
+        start_logging();
+    }
+}
+
+#else // LOGGING_ENABLED
+
+void Tracker::Log_Write_Attitude(void) {}
+void Tracker::Log_Write_Startup() {}
+void Tracker::Log_Write_Baro(void) {}
+
+void Tracker::start_logging() {}
+void Tracker::log_init(void) {}
+
+#endif // LOGGING_ENABLED

AntennaTracker/Parameters.cpp

@@ -226,6 +226,14 @@ const AP_Param::Info Tracker::var_info[] = {
     // @Path: GCS_Mavlink.cpp
     GOBJECTN(gcs[3],  gcs3,       "SR3_",     GCS_MAVLINK),
 
+    // @Param: LOG_BITMASK
+    // @DisplayName: Log bitmask
+    // @Description: 4 byte bitmap of log types to enable
+    // @Values: 63:Default,0:Disabled
+    // @Bitmask: 0:ATTITUDE,1:GPS,2:RCIN,3:IMU,4:RCOUT,5:COMPASS
+    // @User: Standard
+    GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
+
     // @Group: INS_
     // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
     GOBJECT(ins,                    "INS_", AP_InertialSensor),

AntennaTracker/Parameters.h

@@ -93,6 +93,7 @@ public:
         k_param_distance_min,
         k_param_sysid_target,       // 138
         k_param_gcs3,               // stream rates for fourth MAVLink port
+        k_param_log_bitmask,        // 140
 
         //
         // 150: Telemetry control
@@ -148,6 +149,8 @@ public:
     //
     AP_Int8 command_total; // 1 if HOME is set
 
+    AP_Int32 log_bitmask;
+
     // PID controllers
     PID         pidPitch2Srv;
     PID         pidYaw2Srv;

AntennaTracker/Tracker.h

@@ -100,6 +100,11 @@ private:
 
     bool usb_connected = false;
 
+    // has a log download started?
+    bool in_log_download = false;
+    bool logging_started = false;
+    DataFlash_Class DataFlash{FIRMWARE_STRING};
+
     AP_GPS gps;
 
     AP_Baro barometer;
@@ -180,10 +185,16 @@ private:
     int8_t slew_dir = 0;
     uint32_t slew_start_ms = 0;
 
+    // use this to prevent recursion during sensor init
+    bool in_mavlink_delay = false;
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
+    static const struct LogStructure log_structure[];
 
+    void dataflash_periodic(void);
     void one_second_loop();
+    void ten_hz_logging_loop();
     void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
     void send_location(mavlink_channel_t chan);
@@ -243,6 +254,12 @@ private:
     void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
     void init_capabilities(void);
     void compass_cal_update();
+    void Log_Write_Attitude();
+    void Log_Write_Baro(void);
+    void Log_Write_Vehicle_Startup_Messages();
+    void start_logging();
+    void log_init(void);
+    bool should_log(uint32_t mask);
 
 public:
     void mavlink_snoop(const mavlink_message_t* msg);

AntennaTracker/config.h

@@ -57,4 +57,30 @@
  # define DISTANCE_MIN_DEFAULT              5.0f    // do not track targets within 5 meters
 #endif
 
+//
+// Dataflash logging control
+//
+#ifndef LOGGING_ENABLED
+# define LOGGING_ENABLED        ENABLED
+#endif
 
+// Default logging bitmask
+#ifndef DEFAULT_LOG_BITMASK
+ # define DEFAULT_LOG_BITMASK \
+    MASK_LOG_ATTITUDE | \
+    MASK_LOG_GPS | \
+    MASK_LOG_RCIN | \
+    MASK_LOG_IMU | \
+    MASK_LOG_RCOUT | \
+    MASK_LOG_COMPASS
+#endif
+
+/*
+  build a firmware version string.
+  GIT_VERSION comes from Makefile builds
+*/
+#ifndef GIT_VERSION
+#define FIRMWARE_STRING THISFIRMWARE
+#else
+#define FIRMWARE_STRING THISFIRMWARE " (" GIT_VERSION ")"
+#endif

AntennaTracker/defines.h

@@ -26,5 +26,14 @@ enum ServoType {
     SERVO_TYPE_CR=2
 };
 
+//  Logging parameters
+#define MASK_LOG_ATTITUDE               (1<<0)
+#define MASK_LOG_GPS                    (1<<1)
+#define MASK_LOG_RCIN                   (1<<2)
+#define MASK_LOG_IMU                    (1<<3)
+#define MASK_LOG_RCOUT                  (1<<4)
+#define MASK_LOG_COMPASS                (1<<5)
+#define MASK_LOG_ANY                    0xFFFF
+
 #endif // _DEFINES_H
 

AntennaTracker/sensors.cpp

@@ -13,6 +13,9 @@ void Tracker::init_barometer(void)
 void Tracker::update_barometer(void)
 {
     barometer.update();
+    if (should_log(MASK_LOG_IMU)) {
+        Log_Write_Baro();
+    }
 }
 
 
@@ -33,6 +36,9 @@ void Tracker::update_compass(void)
     if (g.compass_enabled && compass.read()) {
         ahrs.set_compass(&compass);
         compass.learn_offsets();
+        if (should_log(MASK_LOG_COMPASS)) {
+            DataFlash.Log_Write_Compass(compass);
+        }
     } else {
         ahrs.set_compass(NULL);
     }
@@ -103,6 +109,11 @@ void Tracker::update_GPS(void)
                 ground_start_count = 0;
             }
         }
+
+        // log GPS data
+        if (should_log(MASK_LOG_GPS)) {
+            DataFlash.Log_Write_GPS(gps, 0, current_loc.alt);
+        }
     }
 }
 

AntennaTracker/system.cpp

@@ -62,6 +62,10 @@ void Tracker::init_tracker()
 
     mavlink_system.sysid = g.sysid_this_mav;
 
+#if LOGGING_ENABLED == ENABLED
+    log_init();
+#endif
+
     if (g.compass_enabled==true) {
         if (!compass.init() || !compass.read()) {
             hal.console->println("Compass initialisation failed!");
@@ -238,3 +242,14 @@ void Tracker::check_usb_mux(void)
     // the user has switched to/from the telemetry port
     usb_connected = usb_check;
 }
+
+/*
+  should we log a message type now?
+ */
+bool Tracker::should_log(uint32_t mask)
+{
+    if (!(mask & g.log_bitmask) || in_mavlink_delay) {
+        return false;
+    }
+    return true;
+}

AntennaTracker/tracking.cpp

@@ -121,6 +121,11 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
     vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
     vehicle.last_update_us = AP_HAL::micros();    
     vehicle.last_update_ms = AP_HAL::millis();
+
+    // log vehicle as GPS2
+    if (should_log(MASK_LOG_GPS)) {
+        DataFlash.Log_Write_GPS(gps, 1, vehicle.location.alt);
+    }
 }