Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: use new atmospheric tables 

and improve pitot handling
This commit is contained in:
Andrew Tridgell 2024-04-28 17:11:56 +10:00 committed by Peter Barker
parent db9cc9ac84
commit 67c506e75c
18 changed files with 76 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/SITL/SIM_Aircraft.cpp
View File

@ -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);
}
}

12
libraries/SITL/SIM_Aircraft.h
View File

@ -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 &currentPos);
// update EAS speeds
void update_eas_airspeed();
private:
uint64_t last_time_us;
uint32_t frame_counter;

5
libraries/SITL/SIM_Airspeed_DLVR.cpp
View File

@ -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);
}

2
libraries/SITL/SIM_Balloon.cpp
View File

@ -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) {

12
libraries/SITL/SIM_DroneCANDevice.cpp
View File

@ -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<uavcan_equipment_air_data_RawAirData> raw_air_pub{CanardInterface::get_test_iface()};
raw_air_pub.broadcast(msg);

7
libraries/SITL/SIM_Frame.cpp
View File

@ -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);
}
/*

9
libraries/SITL/SIM_InertialLabs.cpp
View File

@ -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;

3
libraries/SITL/SIM_JSON.cpp
View File

@ -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

6
libraries/SITL/SIM_MS5525.cpp
View File

@ -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;
}

9
libraries/SITL/SIM_MS5611.cpp
View File

@ -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);

2
libraries/SITL/SIM_MS5XXX.cpp
View File

@ -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;

6
libraries/SITL/SIM_MicroStrain.cpp
View File

@ -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

1
libraries/SITL/SIM_Plane.h
View File

@ -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;

2
libraries/SITL/SIM_Scrimmage.cpp
View File

@ -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;

2
libraries/SITL/SIM_SingleCopter.cpp
View File

@ -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;

5
libraries/SITL/SIM_Temperature_TSYS01.cpp
View File

@ -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;
}

8
libraries/SITL/SIM_VectorNav.cpp
View File

@ -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;

4
libraries/SITL/SITL.h
View File

@ -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