From 2d0bce18a81ee152883369e4186385d343f4de84 Mon Sep 17 00:00:00 2001 From: khancyr Date: Mon, 24 Jul 2017 13:00:36 +0200 Subject: [PATCH] AP_Baro: rename all local variable with prefix fix last missing spaces --- libraries/AP_Baro/AP_Baro_SITL.cpp | 44 +++++++++++++++--------------- libraries/AP_Baro/AP_Baro_SITL.h | 10 +++---- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index f8a0e09e23..dca2708049 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -13,8 +13,8 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : _has_sample(false), AP_Baro_Backend(baro) { - sitl = (SITL::SITL *)AP_Param::find_object("SIM_"); - if (sitl != nullptr) { + _sitl = (SITL::SITL *)AP_Param::find_object("SIM_"); + if (_sitl != nullptr) { _instance = _frontend.register_sensor(); hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Baro_SITL::_timer, void)); } @@ -24,10 +24,10 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : void AP_Baro_SITL::temperature_adjustment(float &p, float &T) { 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 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.0f; // start baro adjustment at 30C T = T1 - (T1 - T0) * expf(-tsec / tconst); if (is_positive(baro_factor)) { @@ -48,42 +48,42 @@ void AP_Baro_SITL::_timer() } _last_sample_time = now; - float sim_alt = sitl->state.altitude; + float sim_alt = _sitl->state.altitude; - if (sitl->baro_disable) { + if (_sitl->baro_disable) { // barometer is disabled return; } - sim_alt += sitl->baro_drift * now / 1000.0f; - sim_alt += sitl->baro_noise * rand_float(); + sim_alt += _sitl->baro_drift * now / 1000.0f; + sim_alt += _sitl->baro_noise * rand_float(); // add baro glitch - sim_alt += sitl->baro_glitch; + sim_alt += _sitl->baro_glitch; // add delay uint32_t best_time_delta = 200; // initialise large time representing buffer entry closest to current time - delay. uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay. // storing data from sensor to buffer - if (now - last_store_time >= 10) { // store data every 10 ms. - last_store_time = now; - if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer - store_index = 0; + if (now - _last_store_time >= 10) { // store data every 10 ms. + _last_store_time = now; + if (_store_index > _buffer_length - 1) { // reset buffer index if index greater than size of buffer + _store_index = 0; } - buffer[store_index].data = sim_alt; // add data to current index - buffer[store_index].time = last_store_time; // add time_stamp to current index - store_index = store_index + 1; // increment index + _buffer[_store_index].data = sim_alt; // add data to current index + _buffer[_store_index].time = _last_store_time; // add time_stamp to current index + _store_index = _store_index + 1; // increment index } // return delayed measurement - const 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++) { + for (uint8_t i = 0; i <= _buffer_length - 1; i++) { // find difference between delayed time and time stamp in buffer uint32_t time_delta = abs( - (int32_t)(delayed_time - buffer[i].time)); + (int32_t)(delayed_time - _buffer[i].time)); // if this difference is smaller than last delta, store this time if (time_delta < best_time_delta) { best_index = i; @@ -91,7 +91,7 @@ void AP_Baro_SITL::_timer() } } if (best_time_delta < 200) { // only output stored state if < 200 msec retrieval error - sim_alt = buffer[best_index].data; + sim_alt = _buffer[best_index].data; } float sigma, delta, theta; diff --git a/libraries/AP_Baro/AP_Baro_SITL.h b/libraries/AP_Baro/AP_Baro_SITL.h index 2e25942a0c..e51ea3f72f 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.h +++ b/libraries/AP_Baro/AP_Baro_SITL.h @@ -18,17 +18,17 @@ protected: private: uint8_t _instance; - SITL::SITL *sitl; + SITL::SITL *_sitl; // barometer delay buffer variables struct readings_baro { uint32_t time; float data; }; - uint8_t store_index; - uint32_t last_store_time; - static const uint8_t buffer_length = 50; - VectorN buffer; + uint8_t _store_index; + uint32_t _last_store_time; + static const uint8_t _buffer_length = 50; + VectorN _buffer; // adjust for simulated board temperature void temperature_adjustment(float &p, float &T);