ardupilot/libraries/SITL/SIM_Airspeed_DLVR.cpp

74 lines
2.1 KiB
C++

#include "SIM_Airspeed_DLVR.h"
#include "SITL.h"
int SITL::Airspeed_DLVR::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
{
struct I2C::i2c_msg &msg = data->msgs[0];
if (msg.flags == I2C_M_RD) {
// driver is attempting to receive reading...
if (msg.len != 4) {
AP_HAL::panic("Unexpected message length (%u)", msg.len);
}
uint8_t status = 0;
if (last_sent_ms == last_update_ms) {
status |= 0b10;
}
last_sent_ms = last_update_ms;
// if (electrical_fault_or_bad_config) {
// status |= 0b11;
// }
// calculation of packed-pressure value:
// this is TYPE_I2C_DLVR_5IN
const uint8_t range_inH2O = 5;
#define DLVR_OFFSET 8192.0f
#define DLVR_SCALE 16384.0f
// const float pressure_pascals = 1555.2f; // maximum transportable
const float press_h2o = pressure * (1.0f/INCH_OF_H2O_TO_PASCAL);
const uint32_t pressure_raw = ((press_h2o / (1.25f * 2.0f * range_inH2O)) * DLVR_SCALE) + DLVR_OFFSET;
// calculation of packed-temperature value
const uint32_t temp_raw = (temperature + 50.0f) * (2047.0f/200.0f);
const uint32_t packed =
(status << 30) |
((pressure_raw & 0x3fff) << 16) |
((temp_raw & 0x7ff) << 5);
msg.buf[0] = (packed >> 24) & 0xff;
msg.buf[1] = (packed >> 16) & 0xff;
msg.buf[2] = (packed >> 8) & 0xff;
msg.buf[3] = (packed >> 0) & 0xff;
return 0;
}
AP_HAL::panic("Should never be written to");
}
void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
{
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_update_ms < 50) { // 20Hz
return;
}
last_update_ms = now_ms;
pressure = AP::sitl()->state.airspeed_raw_pressure[0];
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;
}