Plane: keep an estimate of the rangefinder altitude

only accept data when we have had 10 samples in a row in range at 50Hz

ArduPlane/ArduPlane.pde

@@ -300,9 +300,15 @@ static AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
 static AP_HAL::AnalogSource *rssi_analog_source;
 
 ////////////////////////////////////////////////////////////////////////////////
-// Sonar
+// rangefinder
 ////////////////////////////////////////////////////////////////////////////////
-static RangeFinder sonar;
+static RangeFinder rangefinder;
+
+static struct {
+    bool in_range;
+    float height_estimate;
+    uint8_t in_range_count;
+} rangefinder_state;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Relay
@@ -783,7 +789,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 },
+    { read_rangefinder,       1,    500 },
     { one_second_loop,       50,   1000 },
     { check_long_failsafe,   15,   1000 },
     { read_receiver_rssi,     5,   1000 },

ArduPlane/GCS_Mavlink.pde

@@ -437,14 +437,14 @@ static void NOINLINE send_wind(mavlink_channel_t chan)
 
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
-    if (!sonar.healthy()) {
+    if (!rangefinder.healthy()) {
         // no sonar to report
         return;
     }
     mavlink_msg_rangefinder_send(
         chan,
-        sonar.distance_cm() * 0.01f,
-        sonar.voltage_mv()*0.001f);
+        rangefinder.distance_cm() * 0.01f,
+        rangefinder.voltage_mv()*0.001f);
 }
 
 static void NOINLINE send_current_waypoint(mavlink_channel_t chan)

ArduPlane/Log.pde

@@ -353,6 +353,8 @@ struct PACKED log_Sonar {
     float baro_alt;
     float groundspeed;
     uint8_t throttle;
+    uint8_t count;
+    float height_estimate;
 };
 
 // Write a sonar packet
@@ -361,11 +363,13 @@ static void Log_Write_Sonar()
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
         timestamp   : hal.scheduler->millis(),
-        distance    : (float)sonar.distance_cm(),
-        voltage     : sonar.voltage_mv()*0.001f,
+        distance    : (float)rangefinder.distance_cm(),
+        voltage     : rangefinder.voltage_mv()*0.001f,
         baro_alt    : barometer.get_altitude(),
         groundspeed : gps.ground_speed(),
-        throttle    : (uint8_t)(100 * channel_throttle->norm_output())
+        throttle    : (uint8_t)(100 * channel_throttle->norm_output()),
+        count       : rangefinder_state.in_range_count,
+        height_estimate : rangefinder_state.height_estimate
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -536,7 +540,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { 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" },
+      "SONR", "IffffBBf",   "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,HEst" },
     { LOG_MODE_MSG, sizeof(log_Mode),             
       "MODE", "IMB",         "TimeMS,Mode,ModeNum" },
     { LOG_CURRENT_MSG, sizeof(log_Current),             

ArduPlane/sensors.pde

@@ -8,15 +8,44 @@ static void init_barometer(void)
     gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
 }
 
-static void init_sonar(void)
+static void init_rangefinder(void)
 {
-    sonar.init();
+    rangefinder.init();
 }
 
-// read the sonars
-static void read_sonars(void)
+/*
+  read the rangefinder and update height estimate
+ */
+static void read_rangefinder(void)
 {
-    sonar.update();
+    rangefinder.update();
+
+    uint16_t distance_cm = rangefinder.distance_cm();
+    int16_t max_distance_cm = rangefinder.max_distance_cm();
+    if (rangefinder.healthy() && distance_cm < max_distance_cm) {
+        // correct the range for attitude (multiply by DCM.c.z, which
+        // is cos(roll)*cos(pitch))
+        float corrected_range = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
+        if (rangefinder_state.in_range_count == 0) {
+            // we've just come back into range, start with this value
+            rangefinder_state.height_estimate = corrected_range;
+        } else {
+            // low pass filter to reduce noise. This runs at 50Hz, so
+            // converges fast. We don't want too much filtering
+            // though, as it would introduce lag which would delay flaring
+            rangefinder_state.height_estimate = 0.75f * rangefinder_state.height_estimate + 0.25f * corrected_range;
+        }
+        // we consider ourselves to be fully in range when we have 10
+        // good samples (0.2s)
+        if (rangefinder_state.in_range_count < 10) {
+            rangefinder_state.in_range_count++;
+        } else {
+            rangefinder_state.in_range = true;
+        }
+    } else {
+        rangefinder_state.in_range_count = 0;
+        rangefinder_state.in_range = false;
+    }
 
     if (should_log(MASK_LOG_SONAR))
         Log_Write_Sonar();

ArduPlane/system.pde

@@ -104,8 +104,8 @@ static void init_ardupilot()
     // init baro before we start the GCS, so that the CLI baro test works
     barometer.init();
 
-    // initialise sonar
-    init_sonar();
+    // initialise rangefinder
+    init_rangefinder();
 
     // initialise battery monitoring
     battery.init();