AP_HAL_SITL: SITL of several airspeed sensors

libraries/AP_HAL_SITL/AnalogIn.cpp

@@ -36,6 +36,9 @@ float ADCSource::read_latest() {
 
     case 1:
         return _sitlState->airspeed_pin_value;
+    
+    case 2:
+        return _sitlState->airspeed_2_pin_value;
 
     case 12:
         return _sitlState->current_pin_value;

libraries/AP_HAL_SITL/SITL_State.h

@@ -59,6 +59,7 @@ public:
     // simulated airspeed, sonar and battery monitor
     uint16_t sonar_pin_value;    // pin 0
     uint16_t airspeed_pin_value; // pin 1
+    uint16_t airspeed_2_pin_value; // pin 2
     uint16_t voltage_pin_value;  // pin 13
     uint16_t current_pin_value;  // pin 12
 
@@ -191,6 +192,7 @@ private:
     uint8_t store_index_wind;
     uint32_t last_store_time_wind;
     VectorN<readings_wind,wind_buffer_length> buffer_wind;
+    VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
     uint32_t time_delta_wind;
     uint32_t delayed_time_wind;
 

libraries/AP_HAL_SITL/sitl_airspeed.cpp

@@ -27,11 +27,15 @@ void SITL_State::_update_airspeed(float airspeed)
 {
     const float airspeed_ratio = 1.9936f;
     const float airspeed_offset = 2013.0f;
+    
+    float airspeed2 = airspeed;
 
     // Check sensor failure
     airspeed = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail;
+    airspeed2 = is_zero(_sitl->arspd2_fail) ? airspeed2 : _sitl->arspd2_fail;
     // Add noise
     airspeed = airspeed + (_sitl->arspd_noise * rand_float());
+    airspeed2 = airspeed2 + (_sitl->arspd_noise * rand_float());
 
     if (!is_zero(_sitl->arspd_fail_pressure)) {
         // compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
@@ -39,17 +43,30 @@ void SITL_State::_update_airspeed(float airspeed)
         float tube_pressure = abs(_sitl->arspd_fail_pressure - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure);
         airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / 101325.01576 + 1), 2.0/7.0) - 1.0));
     }
+    if (!is_zero(_sitl->arspd2_fail_pressure)) {
+        // compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
+        // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
+        float tube_pressure = abs(_sitl->arspd2_fail_pressure - _barometer->get_pressure() + _sitl->arspd2_fail_pitot_pressure);
+        airspeed2 = 340.29409348 * sqrt(5 * (pow((tube_pressure / 101325.01576 + 1), 2.0/7.0) - 1.0));
+    }
 
     float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;
+    float airspeed2_pressure = (airspeed2 * airspeed2) / airspeed_ratio;
 
     // flip sign here for simulating reversed pitot/static connections
     if (_sitl->arspd_signflip) airspeed_pressure *= -1;
+    if (_sitl->arspd_signflip) airspeed2_pressure *= -1;
 
     float airspeed_raw = airspeed_pressure + airspeed_offset;
+    float airspeed2_raw = airspeed2_pressure + airspeed_offset;
     if (airspeed_raw / 4 > 0xFFFF) {
         airspeed_pin_value = 0xFFFF;
         return;
     }
+    if (airspeed2_raw / 4 > 0xFFFF) {
+        airspeed_2_pin_value = 0xFFFF;
+        return;
+    }
     // add delay
     const uint32_t now = AP_HAL::millis();
     uint32_t best_time_delta_wind = 200;  // initialise large time representing buffer entry closest to current time - delay.
@@ -63,6 +80,8 @@ void SITL_State::_update_airspeed(float airspeed)
         }
         buffer_wind[store_index_wind].data = airspeed_raw;  // add data to current index
         buffer_wind[store_index_wind].time = last_store_time_wind;  // add time to current index
+        buffer_wind_2[store_index_wind].data = airspeed2_raw;  // add data to current index
+        buffer_wind_2[store_index_wind].time = last_store_time_wind;  // add time to current index
         store_index_wind = store_index_wind + 1;  // increment index
     }
 
@@ -81,9 +100,11 @@ void SITL_State::_update_airspeed(float airspeed)
     }
     if (best_time_delta_wind < 200) {  // only output stored state if < 200 msec retrieval error
         airspeed_raw = buffer_wind[best_index_wind].data;
+        airspeed2_raw = buffer_wind_2[best_index_wind].data;
     }
 
     airspeed_pin_value = airspeed_raw / 4;
+    airspeed_2_pin_value = airspeed2_raw / 4;
 }
 
 #endif