mirror of https://github.com/ArduPilot/ardupilot
79 lines
2.8 KiB
C++
79 lines
2.8 KiB
C++
#include "SIM_MS5525.h"
|
|
|
|
#include <SITL/SITL.h>
|
|
|
|
using namespace SITL;
|
|
|
|
void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
|
|
{
|
|
const uint8_t Q1 = Qx_coeff[0];
|
|
const uint8_t Q2 = Qx_coeff[1];
|
|
const uint8_t Q3 = Qx_coeff[2];
|
|
const uint8_t Q4 = Qx_coeff[3];
|
|
const uint8_t Q5 = Qx_coeff[4];
|
|
const uint8_t Q6 = Qx_coeff[5];
|
|
|
|
// this is the forward conversion copied from the driver:
|
|
float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
|
|
float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
|
|
float OFF = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
|
|
float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
|
|
float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
|
|
const float PSI_to_Pa = 6894.757f;
|
|
P_Pa = PSI_to_Pa * 1.0e-4 * P;
|
|
Temp_C = TEMP * 0.01;
|
|
}
|
|
|
|
void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
|
|
{
|
|
const uint8_t Q1 = Qx_coeff[0];
|
|
const uint8_t Q2 = Qx_coeff[1];
|
|
const uint8_t Q3 = Qx_coeff[2];
|
|
const uint8_t Q4 = Qx_coeff[3];
|
|
const uint8_t Q5 = Qx_coeff[4];
|
|
const uint8_t Q6 = Qx_coeff[5];
|
|
|
|
const float TEMP = Temp_C * 100.0f;
|
|
const float dT = ((TEMP - 2000.0) * (1L<<Q6)) / int64_t(prom[6]);
|
|
const float PSI_to_Pa = 6894.757f;
|
|
const float P = P_Pa / (PSI_to_Pa * 1.0e-4);
|
|
const float OFF = int64_t(prom[2]) * (1L<<Q2) + (int64_t(prom[4]) * dT) / (1L<<Q4);
|
|
const float SENS = int64_t(prom[1]) * (1L<<Q1) + (int64_t(prom[3]) * dT) / (1L<<Q3);
|
|
|
|
D1 = ((P * (1L<<15) + OFF) * (1L<<21)) / SENS;
|
|
D2 = dT + int64_t(prom[5]) * (1L<<Q5);
|
|
}
|
|
|
|
void MS5525::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
|
|
{
|
|
float f_P_Pa;
|
|
float f_Temp_C;
|
|
convert_forward(D1, D2, f_P_Pa, f_Temp_C);
|
|
|
|
const float p_error = fabs(f_P_Pa - P_Pa);
|
|
const float p_error_max = 0.1;
|
|
if (p_error > p_error_max) {
|
|
AP_HAL::panic("Invalid pressure conversion: error %f Pa > %f Pa error max", p_error, p_error_max);
|
|
}
|
|
|
|
const float t_error = fabs(f_Temp_C - Temp_C);
|
|
const float t_error_max = 0.01;
|
|
if (t_error > t_error_max) {
|
|
AP_HAL::panic("Invalid temperature conversion: error %f degC > %f degC error max", t_error, t_error_max);
|
|
}
|
|
}
|
|
|
|
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;
|
|
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;
|
|
}
|