[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.

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;