Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: added MSP backend

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-16 21:35:15 +11:00
parent ae2a310b9c
commit 857d905d9f
5 changed files with 142 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -35,9 +35,12 @@
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include "AP_Airspeed_UAVCAN.h"
#endif
#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
#include "AP_Airspeed_NMEA.h"
#endif
#if HAL_MSP_AIRSPEED_ENABLED
#include "AP_Airspeed_MSP.h"
#endif
extern const AP_HAL::HAL &hal;
#ifdef HAL_AIRSPEED_TYPE_DEFAULT
@ -80,7 +83,7 @@ const AP_Param::GroupInfo AP_Airspeed::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:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
// @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:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP
// @User: Standard
AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE),
@ -187,7 +190,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @Param: 2_TYPE
// @DisplayName: Second Airspeed type
// @Description: Type of 2nd 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:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed
// @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:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP
// @User: Standard
AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE),
@ -274,7 +277,7 @@ AP_Airspeed::AP_Airspeed()
}
void AP_Airspeed::init()
{
{
if (sensor[0] != nullptr) {
// already initialised
return;
@ -361,10 +364,14 @@ void AP_Airspeed::init()
case TYPE_NMEA_WATER:
#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
sensor[i] = new AP_Airspeed_NMEA(*this, i);
#endif
break;
case TYPE_MSP:
#if HAL_MSP_AIRSPEED_ENABLED
sensor[i] = new AP_Airspeed_MSP(*this, i, 0);
#endif
break;
}
if (sensor[i] && !sensor[i]->init()) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u init failed", i + 1);
delete sensor[i];
@ -564,6 +571,25 @@ void AP_Airspeed::update(bool log)
#endif
}
#if HAL_MSP_AIRSPEED_ENABLED
/*
handle MSP airspeed data
*/
void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
{
if (pkt.instance > 1) {
return; //supporting 2 airspeed sensors at most
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (sensor[i]) {
sensor[i]->handle_msp(pkt);
}
}
}
#endif
void AP_Airspeed::Log_Airspeed()
{
const uint64_t now = AP_HAL::micros64();

9
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -5,6 +5,7 @@
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Math/AP_Math.h>
#include <AP_MSP/msp.h>
class AP_Airspeed_Backend;
@ -16,6 +17,9 @@ class AP_Airspeed_Backend;
#define AP_AIRSPEED_AUTOCAL_ENABLE !defined(HAL_BUILD_AP_PERIPH)
#endif
#ifndef HAL_MSP_AIRSPEED_ENABLED
#define HAL_MSP_AIRSPEED_ENABLED HAL_MSP_SENSORS_ENABLED
#endif
class Airspeed_Calibration {
public:
friend class AP_Airspeed;
@ -162,6 +166,7 @@ public:
TYPE_I2C_DLVR_30IN=11,
TYPE_I2C_DLVR_60IN=12,
TYPE_NMEA_WATER=13,
TYPE_MSP=14,
};
// get current primary sensor
@ -179,6 +184,10 @@ public:
float get_corrected_pressure(void) const {
return get_corrected_pressure(primary);
}
#if HAL_MSP_AIRSPEED_ENABLED
void handle_msp(const MSP::msp_airspeed_data_message_t &pkt);
#endif
private:
static AP_Airspeed *_singleton;

4
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -42,6 +42,10 @@ public:
// return airspeed in m/s if available
virtual bool get_airspeed(float& airspeed) {return false;}
#if HAL_MSP_AIRSPEED_ENABLED
virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {}
#endif
protected:
int8_t get_pin(void) const;
float get_psi_range(void) const;

63
libraries/AP_Airspeed/AP_Airspeed_MSP.cpp Normal file
View File

@ -0,0 +1,63 @@
#include "AP_Airspeed_MSP.h"
#if HAL_MSP_AIRSPEED_ENABLED
AP_Airspeed_MSP::AP_Airspeed_MSP(AP_Airspeed &_frontend, uint8_t _instance, uint8_t _msp_instance) :
AP_Airspeed_Backend(_frontend, _instance),
msp_instance(_msp_instance)
{
}
// return the current differential_pressure in Pascal
bool AP_Airspeed_MSP::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_MSP::get_temperature(float &temperature)
{
WITH_SEMAPHORE(sem);
if (temp_count == 0) {
return false;
}
temperature = sum_temp/temp_count;
temp_count = 0;
sum_temp = 0;
return true;
}
void AP_Airspeed_MSP::handle_msp(const MSP::msp_airspeed_data_message_t &pkt)
{
if (pkt.instance != msp_instance) {
// not for us
return;
}
WITH_SEMAPHORE(sem);
sum_pressure += pkt.pressure;
press_count++;
if (press_count > 100) {
// prevent overflow
sum_pressure /= 2;
press_count /= 2;
}
sum_temp += pkt.temp*0.01;
temp_count++;
if (temp_count > 100) {
// prevent overflow
sum_temp /= 2;
temp_count /= 2;
}
}
#endif // HAL_MSP_AIRSPEED_ENABLED

35
libraries/AP_Airspeed/AP_Airspeed_MSP.h Normal file
View File

@ -0,0 +1,35 @@
/*
MSP airspeed backend
*/
#pragma once
#include "AP_Airspeed_Backend.h"
#if HAL_MSP_AIRSPEED_ENABLED
class AP_Airspeed_MSP : public AP_Airspeed_Backend
{
public:
AP_Airspeed_MSP(AP_Airspeed &airspeed, uint8_t instance, uint8_t msp_instance);
bool init(void) override {
return true;
}
void handle_msp(const MSP::msp_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:
const uint8_t msp_instance;
float sum_pressure;
uint8_t press_count;
float sum_temp;
uint8_t temp_count;
};
#endif // HAL_MSP_AIRSPEED_ENABLED