HAL_SITL: moved SITL baro to AP_Baro_SITL

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -87,7 +87,6 @@ void SITL_State::_sitl_setup(const char *home_str)
     if (_sitl != nullptr) {
         // setup some initial values
 #ifndef HIL_MODE
-        _update_barometer(100);
         _update_ins(0);
         _update_compass();
         _update_gps(0, 0, 0, 0, 0, 0, false);
@@ -156,7 +155,6 @@ void SITL_State::_fdm_input_step(void)
 
     if (_update_count == 0 && _sitl != nullptr) {
         _update_gps(0, 0, 0, 0, 0, 0, false);
-        _update_barometer(0);
         _scheduler->timer_event();
         _scheduler->sitl_end_atomic();
         return;
@@ -168,7 +166,6 @@ void SITL_State::_fdm_input_step(void)
                     _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
                     !_sitl->gps_disable);
         _update_ins(_sitl->state.airspeed);
-        _update_barometer(_sitl->state.altitude);
         _update_compass();
 
         if (_sitl->adsb_plane_count >= 0 &&
@@ -433,18 +430,12 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
 }
 
 
-// generate a random float between -1 and 1
-float SITL_State::_rand_float(void)
-{
-    return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
-}
-
 // generate a random Vector3f of size 1
 Vector3f SITL_State::_rand_vec3f(void)
 {
-    Vector3f v = Vector3f(_rand_float(),
-                          _rand_float(),
-                          _rand_float());
+    Vector3f v = Vector3f(rand_float(),
+                          rand_float(),
+                          rand_float());
     if (v.length() != 0.0f) {
         v.normalize();
     }

libraries/AP_HAL_SITL/SITL_State.h

@@ -83,7 +83,6 @@ private:
     void _setup_adc(void);
 
     void set_height_agl(void);
-    void _update_barometer(float height);
     void _update_compass(void);
 
     void _set_signal_handlers(void) const;
@@ -132,7 +131,6 @@ private:
     void _simulator_output(bool synthetic_clock_mode);
     uint16_t _airspeed_sensor(float airspeed);
     uint16_t _ground_sonar();
-    float _rand_float(void);
     Vector3f _rand_vec3f(void);
     void _fdm_input_step(void);
 
@@ -173,7 +171,6 @@ private:
     // delay buffer variables
     static const uint8_t mag_buffer_length = 250;
     static const uint8_t wind_buffer_length = 50;
-    static const uint8_t baro_buffer_length = 50;
 
     // magnetometer delay buffer variables
     struct readings_mag {
@@ -197,17 +194,6 @@ private:
     uint32_t time_delta_wind;
     uint32_t delayed_time_wind;
 
-    // barometer delay buffer variables
-    struct readings_baro {
-        uint32_t time;
-        float data;
-    };
-    uint8_t store_index_baro;
-    uint32_t last_store_time_baro;
-    VectorN<readings_baro,baro_buffer_length> buffer_baro;
-    uint32_t time_delta_baro;
-    uint32_t delayed_time_baro;
-
     // internal SITL model
     SITL::Aircraft *sitl_model;
 

libraries/AP_HAL_SITL/sitl_barometer.cpp

@@ -1,95 +0,0 @@
-/*
-  SITL handling
-
-  This simulates a barometer
-
-  Andrew Tridgell November 2011
- */
-
-#include <AP_HAL/AP_HAL.h>
-#include <AP_Math/AP_Math.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-
-#include "AP_HAL_SITL.h"
-
-using namespace HALSITL;
-
-extern const AP_HAL::HAL& hal;
-
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <cmath>
-
-/*
-  setup the barometer with new input
-  altitude is in meters
- */
-void SITL_State::_update_barometer(float altitude)
-{
-    static uint32_t last_update;
-
-    float sim_alt = altitude;
-
-    if (_barometer == nullptr) {
-        // this sketch doesn't use a barometer
-        return;
-    }
-
-    if (_sitl->baro_disable) {
-        // barometer is disabled
-        return;
-    }
-
-    // 80Hz
-    uint32_t now = AP_HAL::millis();
-    if ((now - last_update) < 12) {
-        return;
-    }
-    last_update = now;
-
-    sim_alt += _sitl->baro_drift * now / 1000;
-    sim_alt += _sitl->baro_noise * _rand_float();
-
-    // add baro glitch
-    sim_alt += _sitl->baro_glitch;
-
-    // add delay
-    uint32_t best_time_delta_baro = 200; // initialise large time representing buffer entry closest to current time - delay.
-    uint8_t best_index_baro = 0; // initialise number representing the index of the entry in buffer closest to delay.
-
-    // storing data from sensor to buffer
-    if (now - last_store_time_baro >= 10) { // store data every 10 ms.
-        last_store_time_baro = now;
-        if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer
-            store_index_baro = 0;
-        }
-        buffer_baro[store_index_baro].data = sim_alt; // add data to current index
-        buffer_baro[store_index_baro].time = last_store_time_baro; // add time_stamp_baro to current index
-        store_index_baro = store_index_baro + 1; // increment index
-    }
-
-    // return delayed measurement
-    delayed_time_baro = now - _sitl->baro_delay; // get time corresponding to delay
-
-    // find data corresponding to delayed time in buffer
-    for (uint8_t i=0; i<=baro_buffer_length-1; i++) {
-        // find difference between delayed time and time stamp in buffer
-        time_delta_baro = abs(
-                (int32_t)(delayed_time_baro - buffer_baro[i].time));
-        // if this difference is smaller than last delta, store this time
-        if (time_delta_baro < best_time_delta_baro) {
-            best_index_baro = i;
-            best_time_delta_baro = time_delta_baro;
-        }
-    }
-    if (best_time_delta_baro < 200) { // only output stored state if < 200 msec retrieval error
-        sim_alt = buffer_baro[best_index_baro].data;
-    }
-
-    _barometer->setHIL(sim_alt);
-}
-
-#endif

libraries/AP_HAL_SITL/sitl_ins.cpp

@@ -107,13 +107,13 @@ uint16_t SITL_State::_ground_sonar(void)
         // adjust for apparent altitude with roll
         altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));
 
-        altitude += _sitl->sonar_noise * _rand_float();
+        altitude += _sitl->sonar_noise * rand_float();
 
         // Altitude in in m, scaler in meters/volt
         voltage = altitude / _sitl->sonar_scale;
         voltage = constrain_float(voltage, 0, 5.0f);
 
-        if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
+        if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) {
             voltage = 5.0f;
         }
     }
@@ -133,7 +133,7 @@ void SITL_State::_update_ins(float airspeed)
 
     sonar_pin_value    = _ground_sonar();
     float airspeed_simulated = (fabsf(_sitl->arspd_fail) > 1.0e-6f) ? _sitl->arspd_fail : airspeed;
-    airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * _rand_float()));
+    airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * rand_float()));
 }
 
 #endif