mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 07:13:56 -04:00
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:
parent
02a7fa5c2b
commit
cc4504e613
@ -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
|
||||
|
@ -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__
|
||||
|
||||
|
@ -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__
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
@ -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__
|
||||
|
||||
|
||||
|
@ -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__
|
||||
|
||||
|
@ -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>
|
||||
|
@ -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__
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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__
|
||||
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -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();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user