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

AP_Airspeed: support external AHRS airspeed sensor

Browse Source
This commit is contained in:
Andrew Tridgell 2023-12-01 13:57:44 +11:00
parent 7102205be3
commit 54ffdc57f2
7 changed files with 137 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -52,6 +52,7 @@
#include "AP_Airspeed_DroneCAN.h"
#include "AP_Airspeed_NMEA.h"
#include "AP_Airspeed_MSP.h"
#include "AP_Airspeed_External.h"
#include "AP_Airspeed_SITL.h"
extern const AP_HAL::HAL &hal;
@ -433,6 +434,11 @@ void AP_Airspeed::allocate()
case TYPE_MSP:
#if AP_AIRSPEED_MSP_ENABLED
sensor[i] = new AP_Airspeed_MSP(*this, i, 0);
#endif
break;
case TYPE_EXTERNAL:
#if AP_AIRSPEED_EXTERNAL_ENABLED
sensor[i] = new AP_Airspeed_External(*this, i);
#endif
break;
}
@ -731,6 +737,24 @@ void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
}
#endif
#if AP_AIRSPEED_EXTERNAL_ENABLED
/*
handle airspeed airspeed data
*/
void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
{
if (!lib_enabled()) {
return;
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (param[i].type == TYPE_EXTERNAL && sensor[i]) {
sensor[i]->handle_external(pkt);
}
}
}
#endif
// @LoggerMessage: HYGR
// @Description: Hygrometer data
// @Field: TimeUS: Time since system startup

8
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -10,6 +10,9 @@
#if AP_AIRSPEED_MSP_ENABLED
#include <AP_MSP/msp.h>
#endif
#if AP_AIRSPEED_EXTERNAL_ENABLED
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#endif
class AP_Airspeed_Backend;
@ -187,6 +190,7 @@ public:
TYPE_NMEA_WATER=13,
TYPE_MSP=14,
TYPE_I2C_ASP5033=15,
TYPE_EXTERNAL=16,
TYPE_SITL=100,
};
@ -208,6 +212,10 @@ public:
void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
#endif
#if AP_AIRSPEED_EXTERNAL_ENABLED
void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt);
#endif
enum class CalibrationState {
NOT_STARTED,
IN_PROGRESS,

3
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -49,6 +49,9 @@ public:
virtual bool get_airspeed(float& airspeed) {return false;}
virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {}
#if AP_AIRSPEED_EXTERNAL_ENABLED
virtual void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) {}
#endif
#if AP_AIRSPEED_HYGROMETER_ENABLE
// optional hygrometer support

58
libraries/AP_Airspeed/AP_Airspeed_External.cpp Normal file
View File

@ -0,0 +1,58 @@
#include "AP_Airspeed_External.h"
#if AP_AIRSPEED_EXTERNAL_ENABLED
AP_Airspeed_External::AP_Airspeed_External(AP_Airspeed &_frontend, uint8_t _instance) :
AP_Airspeed_Backend(_frontend, _instance)
{
set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,_instance,0));
}
// return the current differential_pressure in Pascal
bool AP_Airspeed_External::get_differential_pressure(float &pressure)
{
WITH_SEMAPHORE(sem);
if (press_count == 0) {
return false;
}
pressure = sum_pressure/press_count;
press_count = 0;
sum_pressure = 0;
return true;
}
// get last temperature
bool AP_Airspeed_External::get_temperature(float &temperature)
{
WITH_SEMAPHORE(sem);
if (temperature_count == 0) {
return false;
}
temperature = sum_temperature/temperature_count;
temperature_count = 0;
sum_temperature = 0;
return true;
}
void AP_Airspeed_External::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt)
{
WITH_SEMAPHORE(sem);
sum_pressure += pkt.differential_pressure;
press_count++;
if (press_count > 100) {
// prevent overflow
sum_pressure /= 2;
press_count /= 2;
}
sum_temperature += pkt.temperature;
temperature_count++;
if (temperature_count > 100) {
// prevent overflow
sum_temperature /= 2;
temperature_count /= 2;
}
}
#endif // AP_AIRSPEED_EXTERNAL_ENABLED

38
libraries/AP_Airspeed/AP_Airspeed_External.h Normal file
View File

@ -0,0 +1,38 @@
/*
external AHRS airspeed backend
*/
#pragma once
#include "AP_Airspeed_config.h"
#if AP_AIRSPEED_EXTERNAL_ENABLED
#include "AP_Airspeed_Backend.h"
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
class AP_Airspeed_External : public AP_Airspeed_Backend
{
public:
AP_Airspeed_External(AP_Airspeed &airspeed, uint8_t instance);
bool init(void) override {
return true;
}
void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) override;
// return the current differential_pressure in Pascal
bool get_differential_pressure(float &pressure) override;
// temperature not available via analog backend
bool get_temperature(float &temperature) override;
private:
float sum_pressure;
uint8_t press_count;
float sum_temperature;
uint8_t temperature_count;
};
#endif // AP_AIRSPEED_EXTERNAL_ENABLED

2
libraries/AP_Airspeed/AP_Airspeed_Params.cpp
View File

@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Airspeed type
// @Description: Type of airspeed sensor
// @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL
// @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE),

5
libraries/AP_Airspeed/AP_Airspeed_config.h
View File

@ -3,6 +3,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_MSP/AP_MSP_config.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS_config.h>
#ifndef AP_AIRSPEED_ENABLED
#define AP_AIRSPEED_ENABLED 1
@ -66,3 +67,7 @@
#ifndef AP_AIRSPEED_HYGROMETER_ENABLE
#define AP_AIRSPEED_HYGROMETER_ENABLE (AP_AIRSPEED_ENABLED && BOARD_FLASH_SIZE > 1024)
#endif
#ifndef AP_AIRSPEED_EXTERNAL_ENABLED
#define AP_AIRSPEED_EXTERNAL_ENABLED AP_AIRSPEED_ENABLED && HAL_EXTERNAL_AHRS_ENABLED
#endif