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
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

View File

@ -16,16 +16,18 @@
/*
* 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"
#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
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

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; }
@ -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__

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__

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;
#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];
@ -70,9 +71,7 @@ void AP_Airspeed_I2C::_collect(void)
}
uint8_t status = data[0] & 0xC0;
if (status == 2) {
return;
} else if (status == 3) {
if (status == 2 || status == 3) {
return;
}
@ -102,12 +101,13 @@ void AP_Airspeed_I2C::_collect(void)
}
// 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;
}

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
{
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__

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__

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>

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__

View File

@ -16,12 +16,12 @@
/*
* 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;
@ -45,4 +45,3 @@ bool AP_Airspeed_Analog::get_differential_pressure(float &pressure)
pressure = _source->voltage_average_ratiometric() * VOLTS_TO_PASCAL;
return true;
}

View File

@ -1,18 +1,18 @@
/// -*- 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
@ -29,5 +29,3 @@ private:
const AP_Int8 &_pin;
int8_t _last_pin;
};
#endif // __AP_AIRSPEED_ANALOG_H__

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
*
*/
#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,
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)
0, 0, 0.000001f)
, Q0(0.01f)
, Q1(0.0000005f)
, state(0, 0, 0)
, DT(1)
, aparm(parms)
{
}

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();