aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Airspeed: fix coding style

Browse Source 
- 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
This commit is contained in:
Lucas De Marchi 2016-01-29 09:38:33 -02:00
parent 02a7fa5c2b
commit cc4504e613
12 changed files with 115 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -16,15 +16,17 @@
/*
* APM_Airspeed.cpp - airspeed (pitot) driver
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_ADC/AP_ADC.h>
#include "AP_Airspeed.h"
extern const AP_HAL::HAL& hal;
#include <AP_ADC/AP_ADC.h>
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
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;

42
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -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 <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#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__

6
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -18,9 +18,7 @@
/*
backend driver class for airspeed
*/
#ifndef __AP_AIRSPEED_BACKEND_H__
#define __AP_AIRSPEED_BACKEND_H__
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
@ -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__

68
libraries/AP_Airspeed/AP_Airspeed_I2C.cpp
View File

@ -18,25 +18,26 @@
/*
backend driver for airspeed from a I2C MS4525D0 sensor
*/
#include "AP_Airspeed_I2C.h"
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#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;
}

19
libraries/AP_Airspeed/AP_Airspeed_I2C.h
View File

@ -18,18 +18,17 @@
/*
backend driver for airspeed from I2C
*/
#ifndef __AP_AIRSPEED_I2C_H__
#define __AP_AIRSPEED_I2C_H__
#pragma once
#include <AP_HAL/AP_HAL.h>
#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__

54
libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
backend driver for airspeed from I2C
*/
#ifndef __AP_AIRSPEED_I2C_H__
#define __AP_AIRSPEED_I2C_H__
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
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__

5
libraries/AP_Airspeed/AP_Airspeed_PX4.cpp
View File

@ -23,6 +23,7 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_Airspeed_PX4.h"
#include <drivers/drv_airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <sys/types.h>
@ -30,7 +31,7 @@
#include <fcntl.h>
#include <unistd.h>
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;

8
libraries/AP_Airspeed/AP_Airspeed_PX4.h
View File

@ -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 <AP_HAL/AP_HAL.h>
#include "AP_Airspeed_Backend.h"
@ -44,5 +42,3 @@ private:
uint64_t _last_timestamp;
float _temperature;
};
#endif // __AP_AIRSPEED_PX4_H__

11
libraries/AP_Airspeed/AP_Airspeed_analog.cpp
View File

@ -16,15 +16,15 @@
/*
* analog airspeed driver
*/
#include "AP_Airspeed_analog.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_ADC/AP_ADC.h>
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#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;
}

18
libraries/AP_Airspeed/AP_Airspeed_analog.h
View File

@ -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 <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#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__

57
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -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 <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#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;

4
libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp
View File

@ -15,13 +15,13 @@
*/
/*
* Airspeed.pde - airspeed example sketch
* Airspeed.cpp - airspeed example sketch
*
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_ADC/AP_ADC.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_HAL/AP_HAL.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();