diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 56bb6480f4..c114487ae8 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -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; iinit()) { - 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= 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; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 5840f0b56d..b3638273be 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -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; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index 4885f89562..bcdcc0bd59 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -8,9 +8,11 @@ void AP_Airspeed::check_sensor_failures() { +#ifndef HAL_BUILD_AP_PERIPH for (uint8_t i=0; i_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) { diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h index 98d1329011..828a3b2fc8 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h @@ -42,4 +42,5 @@ private: } _detected_modules[AIRSPEED_MAX_SENSORS]; static HAL_Semaphore _sem_registry; + bool _have_temperature; }; diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 393f764ea7..202404eb75 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -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 }