SITL: use new atmospheric tables

and improve pitot handling

libraries/SITL/SIM_Aircraft.cpp

@@ -630,6 +630,10 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
 
     const float delta_time = frame_time_us * 1.0e-6f;
 
+    // update eas2tas and air density
+    eas2tas = AP_Baro::get_EAS2TAS_for_alt_amsl(location.alt*0.01);
+    air_density = AP_Baro::get_air_density_for_alt_amsl(location.alt*0.01);
+
     // update rotational rates in body frame
     gyro += rot_accel * delta_time;
 
@@ -674,10 +678,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     velocity_air_bf = dcm.transposed() * velocity_air_ef;
 
     // airspeed
-    airspeed = velocity_air_ef.length();
+    update_eas_airspeed();
-
-    // airspeed as seen by a fwd pitot tube (limited to 120m/s)
-    airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1.0f, 0.0f, 0.0f), 0.0f, 120.0f);
 
     // constrain height to the ground
     if (on_ground()) {
@@ -1171,10 +1172,33 @@ Vector3d Aircraft::get_position_relhome() const
 // get air density in kg/m^3
 float Aircraft::get_air_density(float alt_amsl) const
 {
-    float sigma, delta, theta;
+    return AP_Baro::get_air_density_for_alt_amsl(alt_amsl);
-
+}
-    AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta);
+
-
+/*
-    const float air_pressure = SSL_AIR_PRESSURE * delta;
+  update EAS airspeed and pitot speed
-    return air_pressure / (ISA_GAS_CONSTANT * SSL_AIR_TEMPERATURE);
+ */
+void Aircraft::update_eas_airspeed()
+{
+    airspeed = velocity_air_ef.length() / eas2tas;
+
+    /*
+      airspeed as seen by a fwd pitot tube (limited to 120m/s)
+    */
+    airspeed_pitot = airspeed;
+
+    // calculate angle between the local flow vector and a pitot tube aligned with the X body axis
+    const float pitot_aoa =  atan2f(sqrtf(sq(velocity_air_bf.y) + sq(velocity_air_bf.z)), velocity_air_bf.x);
+
+    /*
+      assume the pitot can correctly capture airspeed up to 20 degrees off the nose
+      and follows a cose law outside that range
+    */
+    const float max_pitot_aoa = radians(20);
+    if (pitot_aoa > radians(90)) {
+        airspeed_pitot = 0;
+    } else if (pitot_aoa > max_pitot_aoa) {
+        const float gain_factor = M_PI_2 / (radians(90) - max_pitot_aoa);
+        airspeed_pitot *= cosf((pitot_aoa - max_pitot_aoa) * gain_factor);
+    }
 }

libraries/SITL/SIM_Aircraft.h

@@ -103,6 +103,7 @@ public:
         return velocity_ef;
     }
 
+    // return TAS airspeed in earth frame
     const Vector3f &get_velocity_air_ef(void) const {
         return velocity_air_ef;
     }
@@ -180,15 +181,15 @@ protected:
     Vector3f gyro;                       // rad/s
     Vector3f velocity_ef;                // m/s, earth frame
     Vector3f wind_ef;                    // m/s, earth frame
-    Vector3f velocity_air_ef;            // velocity relative to airmass, earth frame
+    Vector3f velocity_air_ef;            // velocity relative to airmass, earth frame (true airspeed)
     Vector3f velocity_air_bf;            // velocity relative to airmass, body frame
     Vector3d position;                   // meters, NED from origin
     float mass;                          // kg
     float external_payload_mass;         // kg
     Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame
-    float airspeed;                      // m/s, apparent airspeed
+    float airspeed;                      // m/s, EAS airspeed
-    float airspeed_pitot;                // m/s, apparent airspeed, as seen by fwd pitot tube
+    float airspeed_pitot;                // m/s, EAS airspeed, as seen by fwd pitot tube
-    float battery_voltage = 0.0f;
+    float battery_voltage;
     float battery_current;
     float local_ground_level;            // ground level at local position
     bool lock_step_scheduled;
@@ -320,6 +321,9 @@ protected:
     // get local thermal updraft
     float get_local_updraft(const Vector3d &currentPos);
 
+    // update EAS speeds
+    void update_eas_airspeed();
+
 private:
     uint64_t last_time_us;
     uint32_t frame_counter;

libraries/SITL/SIM_Airspeed_DLVR.cpp

@@ -65,9 +65,6 @@ void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
     float sim_alt = AP::sitl()->state.altitude;
     sim_alt += 2 * rand_float();
 
-    float sigma, delta, theta;
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-
     // To Do: Add a sensor board temperature offset parameter
-    temperature = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
+    temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
 }

libraries/SITL/SIM_Balloon.cpp

@@ -50,7 +50,7 @@ void Balloon::update(const struct sitl_input &input)
     Vector3f rot_accel = -gyro * radians(400) / terminal_rotation_rate;
 
     // air resistance
-    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
+    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity) / eas2tas;
 
     float lift_accel = 0;
     if (!burst && released) {

libraries/SITL/SIM_DroneCANDevice.cpp

@@ -92,11 +92,10 @@ void DroneCANDevice::update_baro() {
     }
 
 #if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
-    float sigma, delta, theta;
 
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
+    float p, t_K;
-    float p = SSL_AIR_PRESSURE * delta;
+    AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, t_K);
-    float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
+    float T = KELVIN_TO_C(t_K);
 
     AP_Baro_SITL::temperature_adjustment(p, T);
     T = C_TO_KELVIN(T);
@@ -131,11 +130,8 @@ void DroneCANDevice::update_airspeed() {
     // this was mostly swiped from SIM_Airspeed_DLVR:
     const float sim_alt = AP::sitl()->state.altitude;
 
-    float sigma, delta, theta;
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-
     // To Do: Add a sensor board temperature offset parameter
-    msg.static_air_temperature = SSL_AIR_TEMPERATURE * theta + 25.0;
+    msg.static_air_temperature = C_TO_KELVIN(AP_Baro::get_temperatureC_for_alt_amsl(sim_alt));
 
     static Canard::Publisher<uavcan_equipment_air_data_RawAirData> raw_air_pub{CanardInterface::get_test_iface()};
     raw_air_pub.broadcast(msg);

libraries/SITL/SIM_Frame.cpp

@@ -323,12 +323,7 @@ static Frame supported_frames[] =
 // get air density in kg/m^3
 float Frame::get_air_density(float alt_amsl) const
 {
-    float sigma, delta, theta;
+    return AP_Baro::get_air_density_for_alt_amsl(alt_amsl);
-
-    AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta);
-
-    const float air_pressure = SSL_AIR_PRESSURE * delta;
-    return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN(model.refTempC)));
 }
 
 /*

libraries/SITL/SIM_InertialLabs.cpp

@@ -39,11 +39,12 @@ void InertialLabs::send_packet(void)
     pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5;
     pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5;
 
-    float sigma, delta, theta;
+    float p, t_K;
-    AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta);
+    AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude+rand_float()*0.25, p, t_K);
-    pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5;
+
+    pkt.baro_data.pressure_pa2 = p;
     pkt.baro_data.baro_alt = fdm.altitude;
-    pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
+    pkt.temperature = KELVIN_TO_C(t_K);
 
     pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1;
     pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1;

libraries/SITL/SIM_JSON.cpp

@@ -331,6 +331,9 @@ void JSON::recv_fdm(const struct sitl_input &input)
 
         // airspeed as seen by a fwd pitot tube (limited to 120m/s)
         airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1.0f, 0.0f, 0.0f), 0.0f, 120.0f);
+
+        // airspeed fix for eas2tas
+        update_eas_airspeed();
     }
 
     // Convert from a meters from origin physics to a lat long alt

libraries/SITL/SIM_MS5525.cpp

@@ -68,11 +68,7 @@ void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
     float sim_alt = AP::sitl()->state.altitude;
     sim_alt += 2 * rand_float();
 
-    float sigma, delta, theta;
+    Temp_C = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-
-    // To Do: Add a sensor board temperature offset parameter
-    Temp_C = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
     const uint8_t instance = 0;  // TODO: work out which sensor this is
     P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset;
 }

libraries/SITL/SIM_MS5611.cpp

@@ -106,15 +106,14 @@ void MS5611::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, ui
 
 void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
 {
-    float sigma, delta, theta;
-
     float sim_alt = AP::sitl()->state.altitude;
     sim_alt += 2 * rand_float();
 
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
+    float p, T_K;
-    P_Pa = SSL_AIR_PRESSURE * delta;
+    AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, T_K);
 
-    Temp_C = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta) + AP::sitl()->temp_board_offset;
+    P_Pa = p;
+    Temp_C = KELVIN_TO_C(T_K) + AP::sitl()->temp_board_offset;
 
     // TO DO add in temperature adjustment by inheritting from AP_Baro_SITL_Generic?
     // AP_Baro_SITL::temperature_adjustment(P_Pa, Temp_C);

libraries/SITL/SIM_MS5XXX.cpp

@@ -58,6 +58,8 @@ void MS5XXX::convert_D2()
         // bug in the conversion code.  The simulation can pass in
         // very, very low numbers.  Clamp it.
         P_Pa = 0.1;
+
+        // This should be a simulation error??? Pressure at 86km is 0.374Pa
     }
 
     uint32_t D1;

libraries/SITL/SIM_MicroStrain.cpp

@@ -101,9 +101,9 @@ void MicroStrain::send_imu_packet(void)
     // Add ambient pressure field
     packet.payload[packet.payload_size++] = 0x06; // Ambient Pressure Field Size
     packet.payload[packet.payload_size++] = 0x17; // Descriptor
-    float sigma, delta, theta;
+
-    AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta);
+    float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
-    put_float(packet, SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.1);
+    put_float(packet, pressure_Pa*0.001 + rand_float() * 0.1);
 
     // Add scaled magnetometer field
     packet.payload[packet.payload_size++] = 0x0E; // Scaled Magnetometer Field Size

libraries/SITL/SIM_Plane.h

@@ -41,7 +41,6 @@ public:
 
 protected:
     const float hover_throttle = 0.7f;
-    const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions
     float angle_of_attack;
     float beta;
 

libraries/SITL/SIM_Scrimmage.cpp

@@ -106,7 +106,7 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
 
 
     // velocity relative to air mass, in earth frame TODO
-    velocity_air_ef = velocity_ef;
+    velocity_air_ef = velocity_ef - wind_ef;
 
     // velocity relative to airmass in body frame TODO
     velocity_air_bf = dcm.transposed() * velocity_air_ef;

libraries/SITL/SIM_SingleCopter.cpp

@@ -90,7 +90,7 @@ void SingleCopter::update(const struct sitl_input &input)
     rot_accel.z -= gyro.z * radians(400.0)  / terminal_rotation_rate;
 
     // air resistance
-    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
+    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity) / eas2tas;
 
     // scale thrust to newtons
     thrust *= thrust_scale;

libraries/SITL/SIM_Temperature_TSYS01.cpp

@@ -193,9 +193,6 @@ float SITL::TSYS01::get_sim_temperature() const
     float sim_alt = AP::sitl()->state.altitude;
     sim_alt += 2 * rand_float();
 
-    float sigma, delta, theta;
-    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
-
     // To Do: Add a sensor board temperature offset parameter
-    return (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
+    return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25;
 }

libraries/SITL/SIM_VectorNav.cpp

@@ -94,9 +94,8 @@ void VectorNav::send_packet1(void)
     pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float());
     pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float());
 
-    float sigma, delta, theta;
+    const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
-    AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta);
+    pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
-    pkt.pressure = SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.01;
 
     pkt.mag[0] = fdm.bodyMagField.x*0.001;
     pkt.mag[1] = fdm.bodyMagField.y*0.001;
@@ -153,7 +152,8 @@ void VectorNav::send_packet2(void)
     simulation_timeval(&tv);
 
     pkt.timeGPS = tv.tv_usec * 1000ULL;
-    pkt.temp = 23.5;
+
+    pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude);
     pkt.numGPS1Sats = 19;
     pkt.GPS1Fix = 3;
     pkt.GPS1posLLA[0] = fdm.latitude;

libraries/SITL/SITL.h

@@ -63,8 +63,8 @@ struct sitl_fdm {
     double rollRate, pitchRate, yawRate; // degrees/s in body frame
     double rollDeg, pitchDeg, yawDeg;    // euler angles, degrees
     Quaternion quaternion;
-    double airspeed; // m/s
+    double airspeed; // m/s, EAS
-    Vector3f velocity_air_bf; // velocity relative to airmass, body frame
+    Vector3f velocity_air_bf; // velocity relative to airmass, body frame, TAS
     double battery_voltage; // Volts
     double battery_current; // Amps
     double battery_remaining; // Ah, if non-zero capacity