diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 77e7e3e0aa..bc69002cce 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/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(); - - // 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); + update_eas_airspeed(); // 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; - - AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta); - - const float air_pressure = SSL_AIR_PRESSURE * delta; - return air_pressure / (ISA_GAS_CONSTANT * SSL_AIR_TEMPERATURE); + return AP_Baro::get_air_density_for_alt_amsl(alt_amsl); +} + +/* + update EAS airspeed and pitot speed + */ +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); + } } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index a96e486949..8a8ade5eeb 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/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_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube - float battery_voltage = 0.0f; + float airspeed; // m/s, EAS airspeed + float airspeed_pitot; // m/s, EAS airspeed, as seen by fwd pitot tube + 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 ¤tPos); + // update EAS speeds + void update_eas_airspeed(); + private: uint64_t last_time_us; uint32_t frame_counter; diff --git a/libraries/SITL/SIM_Airspeed_DLVR.cpp b/libraries/SITL/SIM_Airspeed_DLVR.cpp index 4cae7047a4..265185fae4 100644 --- a/libraries/SITL/SIM_Airspeed_DLVR.cpp +++ b/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); } diff --git a/libraries/SITL/SIM_Balloon.cpp b/libraries/SITL/SIM_Balloon.cpp index 77eca0b1ab..218610be8b 100644 --- a/libraries/SITL/SIM_Balloon.cpp +++ b/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) { diff --git a/libraries/SITL/SIM_DroneCANDevice.cpp b/libraries/SITL/SIM_DroneCANDevice.cpp index 598a53a0da..de7d43c451 100644 --- a/libraries/SITL/SIM_DroneCANDevice.cpp +++ b/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 = SSL_AIR_PRESSURE * delta; - float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta); + float p, t_K; + AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, t_K); + 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 raw_air_pub{CanardInterface::get_test_iface()}; raw_air_pub.broadcast(msg); diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 0f9bc5fb62..d5435047f9 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/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; - - 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))); + return AP_Baro::get_air_density_for_alt_amsl(alt_amsl); } /* diff --git a/libraries/SITL/SIM_InertialLabs.cpp b/libraries/SITL/SIM_InertialLabs.cpp index edaadf0b87..aa191f0e9e 100644 --- a/libraries/SITL/SIM_InertialLabs.cpp +++ b/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; - AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta); - pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5; + float p, t_K; + AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude+rand_float()*0.25, p, t_K); + + 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; diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index 5c32227d5d..95b51904c4 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/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 diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp index 24e2000d52..760c4202f9 100644 --- a/libraries/SITL/SIM_MS5525.cpp +++ b/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; - 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; + Temp_C = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt); 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; } diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp index a1957df17d..d205665d5e 100644 --- a/libraries/SITL/SIM_MS5611.cpp +++ b/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); - P_Pa = SSL_AIR_PRESSURE * delta; + float p, T_K; + 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); diff --git a/libraries/SITL/SIM_MS5XXX.cpp b/libraries/SITL/SIM_MS5XXX.cpp index 053a832048..faca67f0bd 100644 --- a/libraries/SITL/SIM_MS5XXX.cpp +++ b/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; diff --git a/libraries/SITL/SIM_MicroStrain.cpp b/libraries/SITL/SIM_MicroStrain.cpp index af4b2c26d6..d56cc87fce 100644 --- a/libraries/SITL/SIM_MicroStrain.cpp +++ b/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); - put_float(packet, SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.1); + + float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + 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 diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 53bb998e46..9b663889b0 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/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; diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp index e4d91548ac..9af2b79991 100644 --- a/libraries/SITL/SIM_Scrimmage.cpp +++ b/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; diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp index d12699c4b8..dc6bd719f4 100644 --- a/libraries/SITL/SIM_SingleCopter.cpp +++ b/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; diff --git a/libraries/SITL/SIM_Temperature_TSYS01.cpp b/libraries/SITL/SIM_Temperature_TSYS01.cpp index 0c1ab789ab..5785a73152 100644 --- a/libraries/SITL/SIM_Temperature_TSYS01.cpp +++ b/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; } diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index ee685ee88a..27c19be7f0 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/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; - AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta); - pkt.pressure = SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.01; + const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + pkt.pressure = pressure_Pa*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; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 9939fb10c0..4d8beb2edd 100644 --- a/libraries/SITL/SITL.h +++ b/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 - Vector3f velocity_air_bf; // velocity relative to airmass, body frame + double airspeed; // m/s, EAS + 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