mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Airspeed: rename MS4525 driver
ready for new driver types
This commit is contained in:
parent
e2192d5b4d
commit
04731dccd6
@ -24,7 +24,7 @@
|
|||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
#include "AP_Airspeed.h"
|
#include "AP_Airspeed.h"
|
||||||
#include "AP_Airspeed_I2C.h"
|
#include "AP_Airspeed_MS4525.h"
|
||||||
#include "AP_Airspeed_analog.h"
|
#include "AP_Airspeed_analog.h"
|
||||||
|
|
||||||
extern const AP_HAL::HAL &hal;
|
extern const AP_HAL::HAL &hal;
|
||||||
@ -143,7 +143,7 @@ void AP_Airspeed::init()
|
|||||||
// nothing to do
|
// nothing to do
|
||||||
break;
|
break;
|
||||||
case TYPE_I2C_MS4525:
|
case TYPE_I2C_MS4525:
|
||||||
sensor = new AP_Airspeed_I2C(*this);
|
sensor = new AP_Airspeed_MS4525(*this);
|
||||||
break;
|
break;
|
||||||
case TYPE_ANALOG:
|
case TYPE_ANALOG:
|
||||||
sensor = new AP_Airspeed_Analog(*this);
|
sensor = new AP_Airspeed_Analog(*this);
|
||||||
|
@ -16,7 +16,7 @@
|
|||||||
/*
|
/*
|
||||||
backend driver for airspeed from a I2C MS4525D0 sensor
|
backend driver for airspeed from a I2C MS4525D0 sensor
|
||||||
*/
|
*/
|
||||||
#include "AP_Airspeed_I2C.h"
|
#include "AP_Airspeed_MS4525.h"
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
@ -35,13 +35,13 @@ extern const AP_HAL::HAL &hal;
|
|||||||
#define MS4525D0_I2C_BUS 1
|
#define MS4525D0_I2C_BUS 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
AP_Airspeed_I2C::AP_Airspeed_I2C(AP_Airspeed &_frontend) :
|
AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend) :
|
||||||
AP_Airspeed_Backend(_frontend)
|
AP_Airspeed_Backend(_frontend)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
// probe and initialise the sensor
|
// probe and initialise the sensor
|
||||||
bool AP_Airspeed_I2C::init()
|
bool AP_Airspeed_MS4525::init()
|
||||||
{
|
{
|
||||||
_dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR);
|
_dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR);
|
||||||
|
|
||||||
@ -63,14 +63,14 @@ bool AP_Airspeed_I2C::init()
|
|||||||
|
|
||||||
if (_last_sample_time_ms != 0) {
|
if (_last_sample_time_ms != 0) {
|
||||||
_dev->register_periodic_callback(20000,
|
_dev->register_periodic_callback(20000,
|
||||||
FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, bool));
|
FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, bool));
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// start a measurement
|
// start a measurement
|
||||||
void AP_Airspeed_I2C::_measure()
|
void AP_Airspeed_MS4525::_measure()
|
||||||
{
|
{
|
||||||
_measurement_started_ms = 0;
|
_measurement_started_ms = 0;
|
||||||
uint8_t cmd = 0;
|
uint8_t cmd = 0;
|
||||||
@ -80,7 +80,7 @@ void AP_Airspeed_I2C::_measure()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// read the values from the sensor
|
// read the values from the sensor
|
||||||
void AP_Airspeed_I2C::_collect()
|
void AP_Airspeed_MS4525::_collect()
|
||||||
{
|
{
|
||||||
uint8_t data[4];
|
uint8_t data[4];
|
||||||
|
|
||||||
@ -129,7 +129,7 @@ void AP_Airspeed_I2C::_collect()
|
|||||||
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
|
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
|
||||||
offset versus voltage for 3 sensors
|
offset versus voltage for 3 sensors
|
||||||
*/
|
*/
|
||||||
void AP_Airspeed_I2C::_voltage_correction(float &diff_press_pa, float &temperature)
|
void AP_Airspeed_MS4525::_voltage_correction(float &diff_press_pa, float &temperature)
|
||||||
{
|
{
|
||||||
const float slope = 65.0f;
|
const float slope = 65.0f;
|
||||||
const float temp_slope = 0.887f;
|
const float temp_slope = 0.887f;
|
||||||
@ -146,7 +146,7 @@ void AP_Airspeed_I2C::_voltage_correction(float &diff_press_pa, float &temperatu
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 50Hz timer
|
// 50Hz timer
|
||||||
bool AP_Airspeed_I2C::_timer()
|
bool AP_Airspeed_MS4525::_timer()
|
||||||
{
|
{
|
||||||
if (_measurement_started_ms == 0) {
|
if (_measurement_started_ms == 0) {
|
||||||
_measure();
|
_measure();
|
||||||
@ -161,7 +161,7 @@ bool AP_Airspeed_I2C::_timer()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return the current differential_pressure in Pascal
|
// return the current differential_pressure in Pascal
|
||||||
bool AP_Airspeed_I2C::get_differential_pressure(float &pressure)
|
bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
|
||||||
{
|
{
|
||||||
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
||||||
return false;
|
return false;
|
||||||
@ -171,7 +171,7 @@ bool AP_Airspeed_I2C::get_differential_pressure(float &pressure)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return the current temperature in degrees C, if available
|
// return the current temperature in degrees C, if available
|
||||||
bool AP_Airspeed_I2C::get_temperature(float &temperature)
|
bool AP_Airspeed_MS4525::get_temperature(float &temperature)
|
||||||
{
|
{
|
||||||
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
|
||||||
return false;
|
return false;
|
@ -27,11 +27,11 @@
|
|||||||
#include "AP_Airspeed_Backend.h"
|
#include "AP_Airspeed_Backend.h"
|
||||||
#include <AP_HAL/I2CDevice.h>
|
#include <AP_HAL/I2CDevice.h>
|
||||||
|
|
||||||
class AP_Airspeed_I2C : public AP_Airspeed_Backend
|
class AP_Airspeed_MS4525 : public AP_Airspeed_Backend
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_Airspeed_I2C(AP_Airspeed &frontend);
|
AP_Airspeed_MS4525(AP_Airspeed &frontend);
|
||||||
~AP_Airspeed_I2C(void) {}
|
~AP_Airspeed_MS4525(void) {}
|
||||||
|
|
||||||
// probe and initialise the sensor
|
// probe and initialise the sensor
|
||||||
bool init() override;
|
bool init() override;
|
Loading…
Reference in New Issue
Block a user