Browse Source

AP_Baro: fix some style and const correctness

mission-4.1.18
khancyr 8 years ago committed by Lucas De Marchi
parent
commit
35ac86ff8d
  1. 18
      libraries/AP_Baro/AP_Baro_SITL.cpp
  2. 2
      libraries/AP_Baro/AP_Baro_SITL.h

18
libraries/AP_Baro/AP_Baro_SITL.cpp

@ -23,14 +23,14 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : @@ -23,14 +23,14 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
// adjust for board temperature
void AP_Baro_SITL::temperature_adjustment(float &p, float &T)
{
float tsec = AP_HAL::millis() * 0.001;
const float tsec = AP_HAL::millis() * 0.001f;
const float T0 = sitl->temp_start;
const float T1 = sitl->temp_flight;
const float tconst = sitl->temp_tconst;
const float baro_factor = sitl->temp_baro_factor;
const float Tzero = 30; // start baro adjustment at 30C
T = T1 - (T1 - T0)*expf(-tsec / tconst);
if (baro_factor > 0) {
const float Tzero = 30.0f; // start baro adjustment at 30C
T = T1 - (T1 - T0) * expf(-tsec / tconst);
if (is_positive(baro_factor)) {
// this produces a pressure change with temperature that
// closely matches what has been observed with a ICM-20789
// barometer. A typical factor is 1.2.
@ -42,7 +42,7 @@ void AP_Baro_SITL::_timer() @@ -42,7 +42,7 @@ void AP_Baro_SITL::_timer()
{
// 100Hz
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
if ((now - _last_sample_time) < 10) {
return;
}
@ -55,7 +55,7 @@ void AP_Baro_SITL::_timer() @@ -55,7 +55,7 @@ void AP_Baro_SITL::_timer()
return;
}
sim_alt += sitl->baro_drift * now / 1000;
sim_alt += sitl->baro_drift * now / 1000.0f;
sim_alt += sitl->baro_noise * rand_float();
// add baro glitch
@ -77,7 +77,7 @@ void AP_Baro_SITL::_timer() @@ -77,7 +77,7 @@ void AP_Baro_SITL::_timer()
}
// return delayed measurement
uint32_t delayed_time = now - sitl->baro_delay; // get time corresponding to delay
const uint32_t delayed_time = now - sitl->baro_delay; // get time corresponding to delay
// find data corresponding to delayed time in buffer
for (uint8_t i=0; i<=buffer_length-1; i++) {
@ -95,9 +95,9 @@ void AP_Baro_SITL::_timer() @@ -95,9 +95,9 @@ void AP_Baro_SITL::_timer()
}
float sigma, delta, theta;
const float p0 = 101325;
const float p0 = 101325.0f;
AP_Baro::SimpleAtmosphere(sim_alt*0.001f, sigma, delta, theta);
AP_Baro::SimpleAtmosphere(sim_alt * 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

2
libraries/AP_Baro/AP_Baro_SITL.h

@ -28,7 +28,7 @@ private: @@ -28,7 +28,7 @@ private:
uint8_t store_index;
uint32_t last_store_time;
static const uint8_t buffer_length = 50;
VectorN<readings_baro,buffer_length> buffer;
VectorN<readings_baro, buffer_length> buffer;
// adjust for simulated board temperature
void temperature_adjustment(float &p, float &T);

Loading…
Cancel
Save