AP_Airspeed: enable for use in AP_Periph

This commit is contained in:
Andrew Tridgell 2019-10-03 21:02:07 +10:00
parent e6db33fddc
commit 167732de0a
6 changed files with 60 additions and 9 deletions

View File

@ -52,6 +52,10 @@ extern const AP_HAL::HAL &hal;
#endif #endif
#endif #endif
#ifndef HAL_AIRSPEED_BUS_DEFAULT
#define HAL_AIRSPEED_BUS_DEFAULT 1
#endif
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#define PSI_RANGE_DEFAULT 0.05 #define PSI_RANGE_DEFAULT 0.05
#endif #endif
@ -60,6 +64,12 @@ extern const AP_HAL::HAL &hal;
#define PSI_RANGE_DEFAULT 1.0f #define PSI_RANGE_DEFAULT 1.0f
#endif #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 // table of user settable parameters
const AP_Param::GroupInfo AP_Airspeed::var_info[] = { const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
@ -97,11 +107,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_PIN", 4, AP_Airspeed, param[0].pin, ARSPD_DEFAULT_PIN), AP_GROUPINFO("_PIN", 4, AP_Airspeed, param[0].pin, ARSPD_DEFAULT_PIN),
#if AP_AIRSPEED_AUTOCAL_ENABLE
// @Param: _AUTOCAL // @Param: _AUTOCAL
// @DisplayName: Automatic airspeed ratio calibration // @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. // @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 // @User: Advanced
AP_GROUPINFO("_AUTOCAL", 5, AP_Airspeed, param[0].autocal, 0), AP_GROUPINFO("_AUTOCAL", 5, AP_Airspeed, param[0].autocal, 0),
#endif
// @Param: _TUBE_ORDER // @Param: _TUBE_ORDER
// @DisplayName: Control pitot 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 // @Description: Bus number of the I2C bus where the airspeed sensor is connected
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced // @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 // @Param: _PRIMARY
// @DisplayName: Primary airspeed sensor // @DisplayName: Primary airspeed sensor
// @Description: This selects which airspeed sensor will be the primary if multiple sensors are found // @Description: This selects which airspeed sensor will be the primary if multiple sensors are found
// @Values: 0:FirstSensor,1:2ndSensor // @Values: 0:FirstSensor,1:2ndSensor
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0), AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0),
#endif
// @Param: _OPTIONS // @Param: _OPTIONS
// @DisplayName: Airspeed options bitmask // @DisplayName: Airspeed options bitmask
@ -143,6 +157,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0), AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0),
#if AIRSPEED_MAX_SENSORS > 1
// @Param: 2_TYPE // @Param: 2_TYPE
// @DisplayName: Second Airspeed type // @DisplayName: Second Airspeed type
// @Description: Type of 2nd airspeed sensor // @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) // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced // @User: Advanced
AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1), 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. // 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() void AP_Airspeed::init()
{ {
if (sensor[0] != nullptr) {
// already initialised
return;
}
// cope with upgrade from old system // cope with upgrade from old system
if (param[0].pin.load() && param[0].pin.get() != 65) { if (param[0].pin.load() && param[0].pin.get() != 65) {
param[0].type.set_default(TYPE_ANALOG); param[0].type.set_default(TYPE_ANALOG);
} }
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
#if AP_AIRSPEED_AUTOCAL_ENABLE
state[i].calibration.init(param[i].ratio); state[i].calibration.init(param[i].ratio);
state[i].last_saved_ratio = 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 // 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; state[i].failures.health_probability = 1.0f;
@ -285,7 +307,7 @@ void AP_Airspeed::init()
} }
if (sensor[i] && !sensor[i]->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]; delete sensor[i];
sensor[i] = nullptr; sensor[i] = nullptr;
} }
@ -326,7 +348,7 @@ bool AP_Airspeed::get_temperature(uint8_t i, float &temperature)
void AP_Airspeed::calibrate(bool in_startup) void AP_Airspeed::calibrate(bool in_startup)
{ {
if (hal.util->was_watchdog_reset()) { 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; return;
} }
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { 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.sum = 0;
state[i].cal.read_count = 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 && if (AP_HAL::millis() - state[i].cal.start_ms >= 1000 &&
state[i].cal.read_count > 15) { state[i].cal.read_count > 15) {
if (state[i].cal.count == 0) { 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 { } 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); param[i].offset.set_and_save(state[i].cal.sum / state[i].cal.count);
} }
state[i].cal.start_ms = 0; state[i].cal.start_ms = 0;
@ -466,9 +488,11 @@ void AP_Airspeed::update(bool log)
check_sensor_failures(); check_sensor_failures();
#ifndef HAL_BUILD_AP_PERIPH
if (log) { if (log) {
Log_Airspeed(); Log_Airspeed();
} }
#endif
} }
void AP_Airspeed::Log_Airspeed() void AP_Airspeed::Log_Airspeed()
@ -515,12 +539,14 @@ bool AP_Airspeed::use(uint8_t i) const
if (!enabled(i) || !param[i].use) { if (!enabled(i) || !param[i].use) {
return false; return false;
} }
#ifndef HAL_BUILD_AP_PERIPH
if (param[i].use == 2 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) { if (param[i].use == 2 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) {
// special case for gliders with airspeed sensors behind the // special case for gliders with airspeed sensors behind the
// propeller. Allow airspeed to be disabled when throttle is // propeller. Allow airspeed to be disabled when throttle is
// running // running
return false; return false;
} }
#endif
return true; return true;
} }

View File

@ -8,7 +8,13 @@
class AP_Airspeed_Backend; class AP_Airspeed_Backend;
#ifndef AIRSPEED_MAX_SENSORS
#define AIRSPEED_MAX_SENSORS 2 #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 { class Airspeed_Calibration {
public: public:
@ -106,7 +112,11 @@ public:
// return health status of sensor // return health status of sensor
bool healthy(uint8_t i) const { 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); } bool healthy(void) const { return healthy(primary); }
@ -180,7 +190,7 @@ private:
float hil_pressure; float hil_pressure;
uint32_t last_update_ms; uint32_t last_update_ms;
bool use_zero_offset; bool use_zero_offset;
// state of runtime calibration // state of runtime calibration
struct { struct {
uint32_t start_ms; uint32_t start_ms;
@ -189,9 +199,11 @@ private:
uint16_t read_count; uint16_t read_count;
} cal; } cal;
#if AP_AIRSPEED_AUTOCAL_ENABLE
Airspeed_Calibration calibration; Airspeed_Calibration calibration;
float last_saved_ratio; float last_saved_ratio;
uint8_t counter; uint8_t counter;
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
struct { struct {
uint32_t last_check_ms; uint32_t last_check_ms;

View File

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

View File

@ -116,7 +116,10 @@ void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id,
if (driver != nullptr) { if (driver != nullptr) {
WITH_SEMAPHORE(driver->_sem_airspeed); WITH_SEMAPHORE(driver->_sem_airspeed);
driver->_pressure = cb.msg->differential_pressure; 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(); 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) bool AP_Airspeed_UAVCAN::get_temperature(float &temperature)
{ {
if (!_have_temperature) {
return false;
}
WITH_SEMAPHORE(_sem_airspeed); WITH_SEMAPHORE(_sem_airspeed);
if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {

View File

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

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) 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) { if (!param[i].autocal) {
// auto-calibration not enabled // auto-calibration not enabled
return; return;
@ -149,6 +150,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t
} else { } else {
state[i].counter++; 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) void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
{ {
#if AP_AIRSPEED_AUTOCAL_ENABLE
const mavlink_airspeed_autocal_t packet{ const mavlink_airspeed_autocal_t packet{
vx: vground.x, vx: vground.x,
vy: vground.y, 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, gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet); (const char *)&packet);
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
} }