[sonar] protect sonar reading by mutex for bebop (#3408)

Both ABI and telemetry messages are not thread-safe and should be
protected by mutex.
Simulation of sonar is handled by NPS.

conf/airframes/test_boards/disco.xml

@@ -7,7 +7,7 @@
   </description>
 
   <firmware name="fixedwing">
-    <configure name="USE_MAGNETOMETER" value="TRUE"/>
+    <configure name="USE_MAGNETOMETER" value="FALSE"/>
 
     <target name="ap" board="disco">
       <configure name="PERIODIC_FREQUENCY" value="120"/>

conf/modules/sonar_bebop.xml

@@ -21,11 +21,10 @@
   </header>
 
   <init fun="sonar_bebop_init()"/>
+  <event fun="sonar_bebop_event()"/>
 
-  <makefile target="ap|sim">
-    <file name="sonar_bebop.c"/>
-  </makefile>
   <makefile target="ap">
+    <file name="sonar_bebop.c"/>
     <define name="USE_SPI0" value="1"/>
     <define name="USE_ADC0" value="1"/>
   </makefile>

sw/airborne/modules/sonar/sonar_bebop.c

@@ -47,10 +47,6 @@
 struct MedianFilterFloat sonar_filt;
 uint32_t sonar_bebop_spike_timer;
 
-#ifdef SITL
-#include "state.h"
-#endif
-
 /** SONAR_BEBOP_INX_DIFF_TO_DIST conversion from index difference to distance based on time of flight
  * ADC speed = 160kHz
  * speed of sound = 340m/s   */
@@ -112,6 +108,8 @@ void *sonar_bebop_read(void *data);
 static float sonar_filter_narrow_obstacles(float distance_sonar);
 #endif
 
+static pthread_mutex_t sonar_mutex = PTHREAD_MUTEX_INITIALIZER;
+
 void sonar_bebop_init(void)
 {
   mode = 0; // default mode is low altitude
@@ -119,6 +117,7 @@ void sonar_bebop_init(void)
 
   sonar_bebop.meas = 0;
   sonar_bebop.offset = 0;
+  sonar_bebop.available = false;
 
   /* configure spi transaction */
   sonar_bebop_spi_t.status        = SPITransDone;
@@ -141,6 +140,36 @@ void sonar_bebop_init(void)
   pthread_setname_np(tid, "sonar");
 #endif
 #endif
+  pthread_mutex_init(&sonar_mutex, NULL);
+}
+
+void sonar_bebop_event(void)
+{
+  pthread_mutex_lock(&sonar_mutex);
+  if (sonar_bebop.available) {
+#if SONAR_COMPENSATE_ROTATION
+    float phi = stateGetNedToBodyEulers_f()->phi;
+    float theta = stateGetNedToBodyEulers_f()->theta;
+    float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
+    sonar_bebop.distance =  sonar_bebop.distance * gain;
+#endif
+
+#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
+    sonar_bebop.distance = sonar_filter_narrow_obstacles(sonar_bebop.distance);
+#endif
+
+    // Send ABI message
+    uint32_t now_ts = get_sys_time_usec();
+    AbiSendMsgAGL(AGL_SONAR_ADC_ID, now_ts, sonar_bebop.distance);
+
+#ifdef SENSOR_SYNC_SEND_SONAR
+    // Send Telemetry report
+    DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
+#endif
+
+    sonar_bebop.available = false;
+  }
+  pthread_mutex_unlock(&sonar_mutex);
 }
 
 uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE];
@@ -150,7 +179,6 @@ uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE];
 void *sonar_bebop_read(void *data __attribute__((unused)))
 {
   while (true) {
-#ifndef SITL
     uint16_t i;
 
     /* Start ADC and send sonar output */
@@ -193,6 +221,7 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
     uint16_t diff = stop_send - start_send;
     if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
         && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
+      pthread_mutex_lock(&sonar_mutex);
       sonar_bebop.meas = (uint16_t)(first_peak - (stop_send - diff / 2));
       sonar_bebop.distance = update_median_filter_f(&sonar_filt, (float)sonar_bebop.meas * SONAR_BEBOP_INX_DIFF_TO_DIST);
 
@@ -210,36 +239,10 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
       } else {
         pulse_transition_counter = 0;
       }
-
+      sonar_bebop.available = true;
-#if SONAR_COMPENSATE_ROTATION
+      pthread_mutex_unlock(&sonar_mutex);
-      float phi = stateGetNedToBodyEulers_f()->phi;
-      float theta = stateGetNedToBodyEulers_f()->theta;
-      float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
-      sonar_bebop.distance =  sonar_bebop.distance * gain;
-#endif
-
-#else // SITL
-      sonar_bebop.distance = stateGetPositionEnu_f()->z;
-      Bound(sonar_bebop.distance, 0.1f, 7.0f);
-      sonar_bebop.meas = (uint16_t)(sonar_bebop.distance / SONAR_BEBOP_INX_DIFF_TO_DIST);
-#endif // SITL
-
-#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
-      sonar_bebop.distance = sonar_filter_narrow_obstacles(sonar_bebop.distance);
-#endif
-
-      // Send ABI message
-      uint32_t now_ts = get_sys_time_usec();
-      AbiSendMsgAGL(AGL_SONAR_ADC_ID, now_ts, sonar_bebop.distance);
-
-#ifdef SENSOR_SYNC_SEND_SONAR
-      // Send Telemetry report
-      DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
-#endif
-
-#ifndef SITL
     }
-#endif
+
     usleep(10000); //100Hz  FIXME: use SYS_TIME_FREQUENCY and divisor
   }
   return NULL;

sw/airborne/modules/sonar/sonar_bebop.h

@@ -33,10 +33,12 @@ struct SonarBebop {
   uint16_t meas;          ///< Raw ADC value
   uint16_t offset;        ///< Sonar offset in ADC units
   float distance;         ///< Distance measured in meters
+  bool available;         ///< New data available
 };
 
 extern struct SonarBebop sonar_bebop;
 
 extern void sonar_bebop_init(void);
+extern void sonar_bebop_event(void);
 
 #endif /* SONAR_BEBOP_H */

sw/simulator/nps/nps_fdm_fixedwing_sim.c

@@ -238,6 +238,7 @@ void nps_fdm_run_step(bool launch, double *commands, int commands_nb __attribute
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
   fdm.hmsl = -fdm.ltpprz_pos.z + NAV_ALT0 / 1000.;
+  fdm.agl = -fdm.ltpprz_pos.z; // flat Earth
 
   /* Eulers */
   fdm.ltp_to_body_eulers = sim_state.attitude;

sw/simulator/nps/nps_sensor_sonar.c

@@ -49,6 +49,13 @@
 #define NPS_SONAR_OFFSET 0
 #endif
 
+#ifndef NPS_SONAR_MIN_DIST
+#define NPS_SONAR_MIN_DIST 0.1f
+#endif
+
+#ifndef NPS_SONAR_MAX_DIST
+#define NPS_SONAR_MAX_DIST 7.0f
+#endif
 
 void nps_sensor_sonar_init(struct NpsSensorSonar *sonar, double time)
 {
@@ -71,6 +78,8 @@ void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time)
   sonar->value = fdm.agl + sonar->offset;
   /* add noise with std dev meters */
   sonar->value += get_gaussian_noise() * sonar->noise_std_dev;
+  /* bound value */
+  Bound(sonar->value, NPS_SONAR_MIN_DIST, NPS_SONAR_MAX_DIST);
 
   sonar->next_update += NPS_SONAR_DT;
   sonar->data_available = TRUE;