From cc4504e613982fce6318edc220b6da51e3891532 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Fri, 29 Jan 2016 09:38:33 -0200 Subject: [PATCH] AP_Airspeed: fix coding style - replace tabs with spaces - remove C-style void from function arguments - use pragma once - fix pointer alignement - remove unused header: AP_Airspeed_I2C_PX4 - we actually use AP_Airspeed_PX4 --- libraries/AP_Airspeed/AP_Airspeed.cpp | 22 +++--- libraries/AP_Airspeed/AP_Airspeed.h | 42 ++++-------- libraries/AP_Airspeed/AP_Airspeed_Backend.h | 6 +- libraries/AP_Airspeed/AP_Airspeed_I2C.cpp | 68 +++++++++---------- libraries/AP_Airspeed/AP_Airspeed_I2C.h | 19 ++---- libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h | 54 --------------- libraries/AP_Airspeed/AP_Airspeed_PX4.cpp | 5 +- libraries/AP_Airspeed/AP_Airspeed_PX4.h | 8 +-- libraries/AP_Airspeed/AP_Airspeed_analog.cpp | 11 ++- libraries/AP_Airspeed/AP_Airspeed_analog.h | 18 +++-- .../AP_Airspeed/Airspeed_Calibration.cpp | 57 ++++++++-------- .../examples/Airspeed/Airspeed.cpp | 4 +- 12 files changed, 115 insertions(+), 199 deletions(-) delete mode 100644 libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index d146f8f7f1..21c13a8859 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -16,15 +16,17 @@ /* * APM_Airspeed.cpp - airspeed (pitot) driver */ - - -#include -#include -#include -#include #include "AP_Airspeed.h" -extern const AP_HAL::HAL& hal; +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +// the virtual pin for digital airspeed sensors +#define AP_AIRSPEED_I2C_PIN 65 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define ARSPD_DEFAULT_PIN 1 @@ -118,7 +120,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { /* - this scaling factor converts from the old system where we used a + this scaling factor converts from the old system where we used a 0 to 4095 raw ADC value for 0-5V to the new system which gets the voltage in volts directly from the ADC driver */ @@ -130,7 +132,7 @@ void AP_Airspeed::init() _calibration.init(_ratio); _last_saved_ratio = _ratio; _counter = 0; - + analog.init(); digital.init(); } @@ -244,7 +246,7 @@ void AP_Airspeed::setHIL(float airspeed, float diff_pressure, float temperature) _raw_airspeed = airspeed; _airspeed = airspeed; _last_pressure = diff_pressure; - _last_update_ms = AP_HAL::millis(); + _last_update_ms = AP_HAL::millis(); _hil_pressure = diff_pressure; _hil_set = true; _healthy = true; diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index af4db761be..38ea6749a9 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -1,17 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_AIRSPEED_H__ -#define __AP_AIRSPEED_H__ +#pragma once #include #include #include -#include #include +#include + #include "AP_Airspeed_Backend.h" -#include "AP_Airspeed_analog.h" -#include "AP_Airspeed_PX4.h" #include "AP_Airspeed_I2C.h" +#include "AP_Airspeed_PX4.h" +#include "AP_Airspeed_analog.h" class Airspeed_Calibration { public: @@ -40,19 +39,10 @@ class AP_Airspeed { public: // constructor - AP_Airspeed(const AP_Vehicle::FixedWing &parms) : - _raw_airspeed(0.0f), - _airspeed(0.0f), - _last_pressure(0.0f), - _raw_pressure(0.0f), - _EAS2TAS(1.0f), - _healthy(false), - _hil_set(false), - _last_update_ms(0), - _calibration(parms), - _last_saved_ratio(0.0f), - _counter(0), - analog(_pin) + AP_Airspeed(const AP_Vehicle::FixedWing &parms) + : _EAS2TAS(1.0f) + , _calibration(parms) + , analog(_pin) { AP_Param::setup_object_defaults(this, var_info); }; @@ -149,7 +139,7 @@ public: // return health status of sensor bool healthy(void) const { return _healthy && fabsf(_offset) > 0; } - void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; }; + void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; } // return time in ms of last update uint32_t last_update_ms(void) const { return _last_update_ms; } @@ -158,9 +148,9 @@ public: static const struct AP_Param::GroupInfo var_info[]; - enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE =0, - PITOT_TUBE_ORDER_NEGATIVE =1, - PITOT_TUBE_ORDER_AUTO =2}; + enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0, + PITOT_TUBE_ORDER_NEGATIVE = 1, + PITOT_TUBE_ORDER_AUTO = 2 }; private: AP_Float _offset; @@ -194,9 +184,3 @@ private: AP_Airspeed_I2C digital; #endif }; - -// the virtual pin for digital airspeed sensors -#define AP_AIRSPEED_I2C_PIN 65 - -#endif // __AP_AIRSPEED_H__ - diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 413fc35097..c672bfd3d0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -18,9 +18,7 @@ /* backend driver class for airspeed */ - -#ifndef __AP_AIRSPEED_BACKEND_H__ -#define __AP_AIRSPEED_BACKEND_H__ +#pragma once #include #include @@ -36,5 +34,3 @@ public: // return the current temperature in degrees C, if available virtual bool get_temperature(float &temperature) = 0; }; - -#endif // __AP_AIRSPEED_BACKEND_H__ diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp index 039218ce73..5c1608c2fa 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp @@ -18,25 +18,26 @@ /* backend driver for airspeed from a I2C MS4525D0 sensor */ +#include "AP_Airspeed_I2C.h" #include #include #include -#include "AP_Airspeed_I2C.h" -extern const AP_HAL::HAL& hal; +extern const AP_HAL::HAL &hal; -#define I2C_ADDRESS_MS4525DO 0x28 +#define I2C_ADDRESS_MS4525DO 0x28 // probe and initialise the sensor -bool AP_Airspeed_I2C::init(void) +bool AP_Airspeed_I2C::init() { // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore - if (!i2c_sem->take(200)) + if (!i2c_sem->take(200)) { return false; + } _measure(); hal.scheduler->delay(10); @@ -50,7 +51,7 @@ bool AP_Airspeed_I2C::init(void) } // start a measurement -void AP_Airspeed_I2C::_measure(void) +void AP_Airspeed_I2C::_measure() { _measurement_started_ms = 0; if (hal.i2c->writeRegisters(I2C_ADDRESS_MS4525DO, 0, 0, NULL) == 0) { @@ -59,7 +60,7 @@ void AP_Airspeed_I2C::_measure(void) } // read the values from the sensor -void AP_Airspeed_I2C::_collect(void) +void AP_Airspeed_I2C::_collect() { uint8_t data[4]; @@ -68,46 +69,45 @@ void AP_Airspeed_I2C::_collect(void) if (hal.i2c->read(I2C_ADDRESS_MS4525DO, 4, data) != 0) { return; } - - uint8_t status = data[0] & 0xC0; - if (status == 2) { + + uint8_t status = data[0] & 0xC0; + if (status == 2 || status == 3) { return; - } else if (status == 3) { - return; - } + } - int16_t dp_raw, dT_raw; - dp_raw = (data[0] << 8) + data[1]; - dp_raw = 0x3FFF & dp_raw; - dT_raw = (data[2] << 8) + data[3]; - dT_raw = (0xFFE0 & dT_raw) >> 5; + int16_t dp_raw, dT_raw; + dp_raw = (data[0] << 8) + data[1]; + dp_raw = 0x3FFF & dp_raw; + dT_raw = (data[2] << 8) + data[3]; + dT_raw = (0xFFE0 & dT_raw) >> 5; - const float P_min = -1.0f; - const float P_max = 1.0f; - const float PSI_to_Pa = 6894.757f; - /* - this equation is an inversion of the equation in the - pressure transfer function figure on page 4 of the datasheet + const float P_min = -1.0f; + const float P_max = 1.0f; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - We negate the result so that positive differential pressures - are generated when the bottom port is used as the static - port on the pitot and top port is used as the dynamic port - */ - float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); - _pressure = diff_press_PSI * PSI_to_Pa; - _temperature = ((200.0f * dT_raw) / 2047) - 50; + _pressure = diff_press_PSI * PSI_to_Pa; + _temperature = ((200.0f * dT_raw) / 2047) - 50; _last_sample_time_ms = AP_HAL::millis(); } // 1kHz timer -void AP_Airspeed_I2C::_timer(void) +void AP_Airspeed_I2C::_timer() { AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - if (!i2c_sem->take_nonblocking()) + if (!i2c_sem->take_nonblocking()) { return; + } if (_measurement_started_ms == 0) { _measure(); @@ -141,5 +141,3 @@ bool AP_Airspeed_I2C::get_temperature(float &temperature) temperature = _temperature; return true; } - - diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.h b/libraries/AP_Airspeed/AP_Airspeed_I2C.h index 0343da3325..15d7205be6 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.h +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.h @@ -18,18 +18,17 @@ /* backend driver for airspeed from I2C */ - -#ifndef __AP_AIRSPEED_I2C_H__ -#define __AP_AIRSPEED_I2C_H__ +#pragma once #include + #include "AP_Airspeed_Backend.h" -class AP_Airspeed_I2C : public AP_Airspeed_Backend +class AP_Airspeed_I2C : public AP_Airspeed_Backend { public: // probe and initialise the sensor - bool init(void); + bool init(); // return the current differential_pressure in Pascal bool get_differential_pressure(float &pressure); @@ -38,15 +37,11 @@ public: bool get_temperature(float &temperature); private: - void _measure(void); - void _collect(void); - void _timer(void); + void _measure(); + void _collect(); + void _timer(); float _temperature; float _pressure; uint32_t _last_sample_time_ms; uint32_t _measurement_started_ms; }; - -#endif // __AP_AIRSPEED_I2C_H__ - - diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h b/libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h deleted file mode 100644 index 44013921c3..0000000000 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h +++ /dev/null @@ -1,54 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -/* - backend driver for airspeed from I2C - */ - -#ifndef __AP_AIRSPEED_I2C_H__ -#define __AP_AIRSPEED_I2C_H__ - -#include -#include - -class AP_Airspeed_I2C_PX4 : AP_Airspeed_I2C { -public: - // constructor - AP_Airspeed_I2C(); - - // probe and initialise the sensor - bool init(void); - - // return the current differential_pressure in Pascal - bool get_differential_pressure(float &pressure); - - // return the current temperature in degrees C, if available - bool get_temperature(float &temperature); - -private: - void _measure(void); - void _collect(void); - void _timer(void); - float _temperature; - float _pressure; - uint32_t _last_sample_time_ms; - uint32_t _measurement_started_ms; -}; - -#endif // __AP_AIRSPEED_I2C_H__ - - diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp b/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp index 924f4a2de0..a263693de4 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp @@ -23,6 +23,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include "AP_Airspeed_PX4.h" + #include #include #include @@ -30,7 +31,7 @@ #include #include -extern const AP_HAL::HAL& hal; +extern const AP_HAL::HAL &hal; bool AP_Airspeed_PX4::init() { @@ -57,7 +58,7 @@ bool AP_Airspeed_PX4::get_differential_pressure(float &pressure) float tsum = 0; uint16_t count = 0; struct differential_pressure_s report; - + while (::read(_fd, &report, sizeof(report)) == sizeof(report) && report.timestamp != _last_timestamp) { psum += report.differential_pressure_raw_pa; diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.h b/libraries/AP_Airspeed/AP_Airspeed_PX4.h index 8b2dcab23e..cd824a53d6 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.h +++ b/libraries/AP_Airspeed/AP_Airspeed_PX4.h @@ -16,11 +16,9 @@ */ /* - backend driver for airspeed from I2C + backend driver for airspeed from PX4Firmware */ - -#ifndef __AP_AIRSPEED_PX4_H__ -#define __AP_AIRSPEED_PX4_H__ +#pragma once #include #include "AP_Airspeed_Backend.h" @@ -44,5 +42,3 @@ private: uint64_t _last_timestamp; float _temperature; }; - -#endif // __AP_AIRSPEED_PX4_H__ diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp index d213fdc59b..8f1b041e84 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp @@ -16,15 +16,15 @@ /* * analog airspeed driver */ +#include "AP_Airspeed_analog.h" - -#include -#include -#include #include +#include +#include + #include "AP_Airspeed.h" -extern const AP_HAL::HAL& hal; +extern const AP_HAL::HAL &hal; // scaling for 3DR analog airspeed sensor #define VOLTS_TO_PASCAL 819 @@ -45,4 +45,3 @@ bool AP_Airspeed_Analog::get_differential_pressure(float &pressure) pressure = _source->voltage_average_ratiometric() * VOLTS_TO_PASCAL; return true; } - diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.h b/libraries/AP_Airspeed/AP_Airspeed_analog.h index cbe02ec9eb..0095adcb22 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.h +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.h @@ -1,19 +1,19 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_AIRSPEED_ANALOG_H__ -#define __AP_AIRSPEED_ANALOG_H__ +#pragma once #include +#include + #include "AP_Airspeed_Backend.h" class AP_Airspeed_Analog : public AP_Airspeed_Backend { public: - AP_Airspeed_Analog(const AP_Int8 &pin) : - _source(NULL), - _pin(pin), - _last_pin(-1) - {} + AP_Airspeed_Analog(const AP_Int8 &pin) + : _source(NULL) + , _pin(pin) + , _last_pin(-1) + { } // probe and initialise the sensor bool init(void); @@ -29,5 +29,3 @@ private: const AP_Int8 &_pin; int8_t _last_pin; }; - -#endif // __AP_AIRSPEED_ANALOG_H__ diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 350022cf21..86d882de3d 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -1,28 +1,29 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* * auto_calibration.cpp - airspeed auto calibration - - * Algorithm by Paul Riseborough + * + * Algorithm by Paul Riseborough * */ +#include #include #include -#include + #include "AP_Airspeed.h" extern const AP_HAL::HAL& hal; // constructor - fill in all the initial values -Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) : - P(100, 0, 0, - 0, 100, 0, - 0, 0, 0.000001f), - Q0(0.01f), - Q1(0.0000005f), - state(0, 0, 0), - DT(1), - aparm(parms) +Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) + : P(100, 0, 0, + 0, 100, 0, + 0, 0, 0.000001f) + , Q0(0.01f) + , Q1(0.0000005f) + , state(0, 0, 0) + , DT(1) + , aparm(parms) { } @@ -47,14 +48,14 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) P.a.x += Q0; P.b.y += Q0; P.c.z += Q1; - + // Perform the predicted measurement using the current state estimates // No state prediction required because states are assumed to be time // invariant plus process noise // Ignore vertical wind component float TAS_pred = state.z * pythagorous3(vg.x - state.x, vg.y - state.y, vg.z); float TAS_mea = airspeed; - + // Calculate the observation Jacobian H_TAS float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x); if (SH1 < 0.000001f) { @@ -68,32 +69,32 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) -(state.z*SH2*(2*vg.x - 2*state.x))/2, -(state.z*SH2*(2*vg.y - 2*state.y))/2, 1/SH2); - + // Calculate the fusion innovaton covariance assuming a TAS measurement // noise of 1.0 m/s // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1] Vector3f PH = P * H_TAS; float S = H_TAS * PH + 1.0f; - + // Calculate the Kalman gain // [3 x 3] * [3 x 1] / [1 x 1] - Vector3f KG = PH / S; - + Vector3f KG = PH / S; + // Update the states state += KG*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1] - + // Update the covariance matrix Vector3f HP2 = H_TAS * P; P -= KG.mul_rowcol(HP2); - + // force symmetry on the covariance matrix - necessary due to rounding // errors - float P12 = 0.5f * (P.a.y + P.b.x); - float P13 = 0.5f * (P.a.z + P.c.x); - float P23 = 0.5f * (P.b.z + P.c.y); - P.a.y = P.b.x = P12; - P.a.z = P.c.x = P13; - P.b.z = P.c.y = P23; + float P12 = 0.5f * (P.a.y + P.b.x); + float P13 = 0.5f * (P.a.z + P.c.x); + float P23 = 0.5f * (P.b.z + P.c.y); + P.a.y = P.b.x = P12; + P.a.z = P.c.x = P13; + P.b.z = P.c.y = P23; // Constrain diagonals to be non-negative - protects against rounding errors P.a.x = MAX(P.a.x, 0.0f); @@ -120,7 +121,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground) // set state.z based on current ratio, this allows the operator to // override the current ratio in flight with autocal, which is - // very useful both for testing and to force a reasonable value. + // very useful both for testing and to force a reasonable value. float ratio = constrain_float(_ratio, 1.0f, 4.0f); _calibration.state.z = 1.0f / sqrtf(ratio); @@ -139,7 +140,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground) zratio = constrain_float(zratio, 0.5f, 1.0f); _ratio.set(1/sq(zratio)); if (_counter > 60) { - if (_last_saved_ratio > 1.05f*_ratio || + if (_last_saved_ratio > 1.05f*_ratio || _last_saved_ratio < 0.95f*_ratio) { _ratio.save(); _last_saved_ratio = _ratio; diff --git a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp index e9a52e42a4..206e71b6a2 100644 --- a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp +++ b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp @@ -15,13 +15,13 @@ */ /* - * Airspeed.pde - airspeed example sketch + * Airspeed.cpp - airspeed example sketch * */ -#include #include #include +#include const AP_HAL::HAL& hal = AP_HAL::get_HAL();