Plane: added sonar logging to dataflash

ArduPlane/ArduPlane.pde

@@ -307,6 +307,11 @@ static AP_HAL::AnalogSource *rssi_analog_source;
 
 static AP_HAL::AnalogSource *vcc_pin;
 
+////////////////////////////////////////////////////////////////////////////////
+// Sonar
+////////////////////////////////////////////////////////////////////////////////
+static AP_RangeFinder_analog sonar;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Relay
 ////////////////////////////////////////////////////////////////////////////////
@@ -728,6 +733,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { compass_accumulate,     1,   1500 },
     { barometer_accumulate,   1,    900 },
     { update_notify,          1,    300 },
+    { read_sonars,            1,    500 },
     { one_second_loop,       50,   3900 },
     { check_long_failsafe,   15,   1000 },
     { airspeed_ratio_update, 50,   1000 },

ArduPlane/Log.pde

@@ -52,6 +52,7 @@ print_log_menu(void)
         PLOG(TECS);
         PLOG(CAMERA);
         PLOG(RC);
+        PLOG(SONAR);
  #undef PLOG
     }
 
@@ -144,6 +145,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
         TARG(TECS);
         TARG(CAMERA);
         TARG(RC);
+        TARG(SONAR);
  #undef TARG
     }
 
@@ -389,6 +391,31 @@ static void Log_Write_Mode(uint8_t mode)
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+struct PACKED log_Sonar {
+    LOG_PACKET_HEADER;
+    uint32_t timestamp;
+    float distance;
+    float voltage;
+    float baro_alt;
+    float groundspeed;
+    uint8_t throttle;
+};
+
+// Write a sonar packet
+static void Log_Write_Sonar()
+{
+    struct log_Sonar pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
+        timestamp   : hal.scheduler->millis(),
+        distance    : sonar.distance_cm(),
+        voltage     : sonar.voltage(),
+        baro_alt    : barometer.get_altitude(),
+        groundspeed : (0.01f * g_gps->ground_speed_cm),
+        throttle    : (uint8_t)(100 * channel_throttle->norm_output())
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
 struct PACKED log_Current {
     LOG_PACKET_HEADER;
     uint32_t time_ms;
@@ -474,6 +501,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "CTUN", "Icccchhf",    "TimeMS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
       "NTUN", "ICICCccfI",   "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
+    { LOG_SONAR_MSG, sizeof(log_Sonar),             
+      "SONR", "IffffB",   "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr" },
     { LOG_MODE_MSG, sizeof(log_Mode),             
       "MODE", "IMB",         "TimeMS,Mode,ModeNum" },
     { LOG_CURRENT_MSG, sizeof(log_Current),             

ArduPlane/Parameters.h

@@ -98,6 +98,7 @@ public:
         k_param_ground_steer_dps,
         k_param_rally_limit_km,
         k_param_hil_err_limit,
+        k_param_sonar,
 
         // 110: Telemetry control
         //

ArduPlane/Parameters.pde

@@ -776,6 +776,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),
 
+    // @Group: SONAR_
+    // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
+    GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),
+
     // RC channel
     //-----------
     // @Group: RC1_

ArduPlane/defines.h

@@ -162,6 +162,7 @@ enum log_messages {
     LOG_COMPASS_MSG,
     LOG_TECS_MSG,
     LOG_RC_MSG,
+    LOG_SONAR_MSG,
     MAX_NUM_LOGS // always at the end
 };
 
@@ -179,6 +180,7 @@ enum log_messages {
 #define MASK_LOG_TECS                   (1<<11)
 #define MASK_LOG_CAMERA                 (1<<12)
 #define MASK_LOG_RC                     (1<<13)
+#define MASK_LOG_SONAR                  (1<<14)
 
 // Waypoint Modes
 // ----------------

ArduPlane/sensors.pde

@@ -23,6 +23,27 @@ static int32_t read_barometer(void)
     return altitude_filter.apply(barometer.get_altitude() * 100.0);
 }
 
+static void init_sonar(void)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+    sonar.Init(&adc);
+#else
+    sonar.Init(NULL);
+#endif
+}
+
+// read the sonars
+static void read_sonars(void)
+{
+    if (!sonar.enabled()) {
+        // this makes it possible to disable sonar at runtime
+        return;
+    }
+
+    if (g.log_bitmask & MASK_LOG_SONAR)
+        Log_Write_Sonar();
+}
+
 /*
   ask airspeed sensor for a new value
  */
@@ -71,3 +92,4 @@ static int32_t adjusted_altitude_cm(void)
 {
     return current_loc.alt - (g.alt_offset*100);
 }
+

ArduPlane/system.pde

@@ -104,6 +104,9 @@ static void init_ardupilot()
     // init baro before we start the GCS, so that the CLI baro test works
     barometer.init();
 
+    // initialise sonar
+    init_sonar();
+
     // init the GCS
     gcs[0].init(hal.uartA);
     // Register mavlink_delay_cb, which will run anytime you have