Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: enable for use in AP_Periph

This commit is contained in:
Andrew Tridgell 2019-10-03 21:02:07 +10:00
parent fa6b6140e3
commit f51378f2e8
6 changed files with 60 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -52,6 +52,10 @@ extern const AP_HAL::HAL &hal;
#endif
#endif
#ifndef HAL_AIRSPEED_BUS_DEFAULT
#define HAL_AIRSPEED_BUS_DEFAULT 1
#endif
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#define PSI_RANGE_DEFAULT 0.05
#endif
@ -60,6 +64,12 @@ extern const AP_HAL::HAL &hal;
#define PSI_RANGE_DEFAULT 1.0f
#endif
#ifdef HAL_NO_GCS
#define GCS_SEND_TEXT(severity, format, args...)
#else
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
#endif
// table of user settable parameters
const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
@ -97,11 +107,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_PIN", 4, AP_Airspeed, param[0].pin, ARSPD_DEFAULT_PIN),
#if AP_AIRSPEED_AUTOCAL_ENABLE
// @Param: _AUTOCAL
// @DisplayName: Automatic airspeed ratio calibration
// @Description: Enables automatic adjustment of ARSPD_RATIO during a calibration flight based on estimation of ground speed and true airspeed. New ratio saved every 2 minutes if change is > 5%. Should not be left enabled.
// @User: Advanced
AP_GROUPINFO("_AUTOCAL", 5, AP_Airspeed, param[0].autocal, 0),
#endif
// @Param: _TUBE_ORDER
// @DisplayName: Control pitot tube order
@ -127,14 +139,16 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @Description: Bus number of the I2C bus where the airspeed sensor is connected
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced
AP_GROUPINFO("_BUS", 9, AP_Airspeed, param[0].bus, 1),
AP_GROUPINFO("_BUS", 9, AP_Airspeed, param[0].bus, HAL_AIRSPEED_BUS_DEFAULT),
#if AIRSPEED_MAX_SENSORS > 1
// @Param: _PRIMARY
// @DisplayName: Primary airspeed sensor
// @Description: This selects which airspeed sensor will be the primary if multiple sensors are found
// @Values: 0:FirstSensor,1:2ndSensor
// @User: Advanced
AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0),
#endif
// @Param: _OPTIONS
// @DisplayName: Airspeed options bitmask
@ -143,6 +157,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0),
#if AIRSPEED_MAX_SENSORS > 1
// @Param: 2_TYPE
// @DisplayName: Second Airspeed type
// @Description: Type of 2nd airspeed sensor
@ -208,6 +223,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced
AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1),
#endif // AIRSPEED_MAX_SENSORS
// Note that 21 is used above by the _OPTIONS parameter. Do not use 21.
@ -233,14 +249,20 @@ AP_Airspeed::AP_Airspeed()
void AP_Airspeed::init()
{
if (sensor[0] != nullptr) {
// already initialised
return;
}
// cope with upgrade from old system
if (param[0].pin.load() && param[0].pin.get() != 65) {
param[0].type.set_default(TYPE_ANALOG);
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
#if AP_AIRSPEED_AUTOCAL_ENABLE
state[i].calibration.init(param[i].ratio);
state[i].last_saved_ratio = param[i].ratio;
#endif
// Set the enable automatically to false and set the probability that the airspeed is healhy to start with
state[i].failures.health_probability = 1.0f;
@ -285,7 +307,7 @@ void AP_Airspeed::init()
}
if (sensor[i] && !sensor[i]->init()) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed %u init failed", i + 1);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u init failed", i + 1);
delete sensor[i];
sensor[i] = nullptr;
}
@ -326,7 +348,7 @@ bool AP_Airspeed::get_temperature(uint8_t i, float &temperature)
void AP_Airspeed::calibrate(bool in_startup)
{
if (hal.util->was_watchdog_reset()) {
gcs().send_text(MAV_SEVERITY_INFO,"Airspeed: skipping cal");
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed: skipping cal");
return;
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
@ -345,7 +367,7 @@ void AP_Airspeed::calibrate(bool in_startup)
state[i].cal.sum = 0;
state[i].cal.read_count = 0;
}
gcs().send_text(MAV_SEVERITY_INFO,"Airspeed calibration started");
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Airspeed calibration started");
}
/*
@ -362,9 +384,9 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
if (AP_HAL::millis() - state[i].cal.start_ms >= 1000 &&
state[i].cal.read_count > 15) {
if (state[i].cal.count == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed %u unhealthy", i + 1);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u unhealthy", i + 1);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u calibrated", i + 1);
param[i].offset.set_and_save(state[i].cal.sum / state[i].cal.count);
}
state[i].cal.start_ms = 0;
@ -466,9 +488,11 @@ void AP_Airspeed::update(bool log)
check_sensor_failures();
#ifndef HAL_BUILD_AP_PERIPH
if (log) {
Log_Airspeed();
}
#endif
}
void AP_Airspeed::Log_Airspeed()
@ -515,12 +539,14 @@ bool AP_Airspeed::use(uint8_t i) const
if (!enabled(i) || !param[i].use) {
return false;
}
#ifndef HAL_BUILD_AP_PERIPH
if (param[i].use == 2 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) {
// special case for gliders with airspeed sensors behind the
// propeller. Allow airspeed to be disabled when throttle is
// running
return false;
}
#endif
return true;
}

16
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -8,7 +8,13 @@
class AP_Airspeed_Backend;
#ifndef AIRSPEED_MAX_SENSORS
#define AIRSPEED_MAX_SENSORS 2
#endif
#ifndef AP_AIRSPEED_AUTOCAL_ENABLE
#define AP_AIRSPEED_AUTOCAL_ENABLE !defined(HAL_BUILD_AP_PERIPH)
#endif
class Airspeed_Calibration {
public:
@ -106,7 +112,11 @@ public:
// return health status of sensor
bool healthy(uint8_t i) const {
return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i);
bool ok = state[i].healthy && enabled(i);
#ifndef HAL_BUILD_AP_PERIPH
ok &= (fabsf(param[i].offset) > 0 || state[i].use_zero_offset);
#endif
return ok;
}
bool healthy(void) const { return healthy(primary); }
@ -180,7 +190,7 @@ private:
float hil_pressure;
uint32_t last_update_ms;
bool use_zero_offset;
// state of runtime calibration
struct {
uint32_t start_ms;
@ -189,9 +199,11 @@ private:
uint16_t read_count;
} cal;
#if AP_AIRSPEED_AUTOCAL_ENABLE
Airspeed_Calibration calibration;
float last_saved_ratio;
uint8_t counter;
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
struct {
uint32_t last_check_ms;

2
libraries/AP_Airspeed/AP_Airspeed_Health.cpp
View File

@ -8,9 +8,11 @@
void AP_Airspeed::check_sensor_failures()
{
#ifndef HAL_BUILD_AP_PERIPH
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
check_sensor_ahrs_wind_max_failures(i);
}
#endif
}
void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)

8
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp
View File

@ -116,7 +116,10 @@ void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id,
if (driver != nullptr) {
WITH_SEMAPHORE(driver->_sem_airspeed);
driver->_pressure = cb.msg->differential_pressure;
driver->_temperature = cb.msg->static_air_temperature - C_TO_KELVIN;
if (!isnan(cb.msg->static_air_temperature)) {
driver->_temperature = cb.msg->static_air_temperature - C_TO_KELVIN;
driver->_have_temperature = true;
}
driver->_last_sample_time_ms = AP_HAL::millis();
}
}
@ -142,6 +145,9 @@ bool AP_Airspeed_UAVCAN::get_differential_pressure(float &pressure)
bool AP_Airspeed_UAVCAN::get_temperature(float &temperature)
{
if (!_have_temperature) {
return false;
}
WITH_SEMAPHORE(_sem_airspeed);
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {

1
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h
View File

@ -42,4 +42,5 @@ private:
} _detected_modules[AIRSPEED_MAX_SENSORS];
static HAL_Semaphore _sem_registry;
bool _have_temperature;
};

4
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -114,6 +114,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m
*/
void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal)
{
#if AP_AIRSPEED_AUTOCAL_ENABLE
if (!param[i].autocal) {
// auto-calibration not enabled
return;
@ -149,6 +150,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t
} else {
state[i].counter++;
}
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}
/*
@ -165,6 +167,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
{
#if AP_AIRSPEED_AUTOCAL_ENABLE
const mavlink_airspeed_autocal_t packet{
vx: vground.x,
vy: vground.y,
@ -181,4 +184,5 @@ void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet);
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}