mirror of https://github.com/ArduPilot/ardupilot
96 lines
2.7 KiB
C++
96 lines
2.7 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#include "AP_Baro_HIL.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_Baro_HIL::AP_Baro_HIL(AP_Baro &baro) :
|
|
AP_Baro_Backend(baro)
|
|
{
|
|
_instance = _frontend.register_sensor();
|
|
}
|
|
|
|
// ==========================================================================
|
|
// based on tables.cpp from http://www.pdas.com/atmosdownload.html
|
|
|
|
/*
|
|
Compute the temperature, density, and pressure in the standard atmosphere
|
|
Correct to 20 km. Only approximate thereafter.
|
|
*/
|
|
void AP_Baro::SimpleAtmosphere(
|
|
const float alt, // geometric altitude, km.
|
|
float& sigma, // density/sea-level standard density
|
|
float& delta, // pressure/sea-level standard pressure
|
|
float& theta) // temperature/sea-level standard temperature
|
|
{
|
|
const float REARTH = 6369.0f; // radius of the Earth (km)
|
|
const float GMR = 34.163195f; // gas constant
|
|
float h=alt*REARTH/(alt+REARTH); // geometric to geopotential altitude
|
|
|
|
if (h < 11.0f) {
|
|
// Troposphere
|
|
theta=(288.15f-6.5f*h)/288.15f;
|
|
delta=powf(theta, GMR/6.5f);
|
|
} else {
|
|
// Stratosphere
|
|
theta=216.65f/288.15f;
|
|
delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
|
|
}
|
|
|
|
sigma = delta/theta;
|
|
}
|
|
|
|
|
|
/*
|
|
convert an altitude in meters above sea level to a presssure and temperature
|
|
*/
|
|
void AP_Baro::setHIL(float altitude_msl)
|
|
{
|
|
float sigma, delta, theta;
|
|
const float p0 = 101325;
|
|
|
|
SimpleAtmosphere(altitude_msl*0.001f, sigma, delta, theta);
|
|
float p = p0 * delta;
|
|
float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin
|
|
|
|
setHIL(0, p, T);
|
|
}
|
|
|
|
/*
|
|
set HIL pressure and temperature for an instance
|
|
*/
|
|
void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature)
|
|
{
|
|
if (instance >= _num_sensors) {
|
|
// invalid
|
|
return;
|
|
}
|
|
_hil.press_buffer.push_back(pressure);
|
|
_hil.temp_buffer.push_back(temperature);
|
|
}
|
|
|
|
// Read the sensor
|
|
void AP_Baro_HIL::update(void)
|
|
{
|
|
float pressure = 0.0;
|
|
float temperature = 0.0;
|
|
float pressure_sum = 0.0;
|
|
float temperature_sum = 0.0;
|
|
uint32_t sum_count = 0;
|
|
|
|
while (_frontend._hil.press_buffer.is_empty() == false){
|
|
_frontend._hil.press_buffer.pop_front(pressure);
|
|
pressure_sum += pressure; // Pressure in Pascals
|
|
_frontend._hil.temp_buffer.pop_front(temperature);
|
|
temperature_sum += temperature; // degrees celcius
|
|
sum_count++;
|
|
}
|
|
|
|
if (sum_count > 0) {
|
|
pressure_sum /= (float)sum_count;
|
|
temperature_sum /= (float)sum_count;
|
|
_copy_to_frontend(0, pressure_sum, temperature_sum);
|
|
}
|
|
}
|