Copter: add beacon data flash logging

Added to Copter vehicle because currently only used by Copter

ArduCopter/ArduCopter.cpp

@@ -406,6 +406,7 @@ void Copter::ten_hz_logging_loop()
     if (should_log(MASK_LOG_CTUN)) {
         attitude_control.control_monitor_log();
         Log_Write_Proximity();
+        Log_Write_Beacon();
     }
 #if FRAME_CONFIG == HELI_FRAME
     Log_Write_Heli();

ArduCopter/Copter.h

@@ -753,6 +753,7 @@ private:
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
     void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok);
     void Log_Write_Proximity();
+    void Log_Write_Beacon();
     void Log_Write_Vehicle_Startup_Messages();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void start_logging() ;

ArduCopter/Log.cpp

@@ -806,6 +806,50 @@ void Copter::Log_Write_Proximity()
 #endif
 }
 
+// beacon sensor logging
+struct PACKED log_Beacon {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t health;
+    uint8_t count;
+    float dist0;
+    float dist1;
+    float dist2;
+    float dist3;
+    float posx;
+    float posy;
+    float posz;
+};
+
+// Write beacon position and distances
+void Copter::Log_Write_Beacon()
+{
+    // exit immediately if feature is disabled
+    if (!g2.beacon.enabled()) {
+        return;
+    }
+
+    // position
+    Vector3f pos;
+    float accuracy = 0.0f;
+    g2.beacon.get_vehicle_position_ned(pos, accuracy);
+
+    struct log_Beacon pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
+        time_us         : AP_HAL::micros64(),
+        health          : (uint8_t)g2.beacon.healthy(),
+        count           : (uint8_t)g2.beacon.count(),
+        dist0           : g2.beacon.beacon_distance(0),
+        dist1           : g2.beacon.beacon_distance(1),
+        dist2           : g2.beacon.beacon_distance(2),
+        dist3           : g2.beacon.beacon_distance(3),
+        posx            : pos.x,
+        posy            : pos.y,
+        posz            : pos.z
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
 const struct LogStructure Copter::log_structure[] = {
     LOG_COMMON_STRUCTURES,
 #if AUTOTUNE_ENABLED == ENABLED
@@ -850,6 +894,8 @@ const struct LogStructure Copter::log_structure[] = {
       "THRO",  "QBffffbbbb",  "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk" },
     { LOG_PROXIMITY_MSG, sizeof(log_Proximity),
       "PRX",   "QBffffffffff","TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315,CAng,CDist" },
+    { LOG_BEACON_MSG, sizeof(log_Beacon),
+      "BCN",   "QBBfffffff",  "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ" },
 };
 
 #if CLI_ENABLED == ENABLED

ArduCopter/defines.h

@@ -308,6 +308,7 @@ enum DevOptions {
 #define LOG_GUIDEDTARGET_MSG            0x22
 #define LOG_THROW_MSG                   0x23
 #define LOG_PROXIMITY_MSG               0x24
+#define LOG_BEACON_MSG                  0x25
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)