AP_Airspeed: enable for use in AP_Periph

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; 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;
 }
 

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;

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<AIRSPEED_MAX_SENSORS; i++) {
         check_sensor_ahrs_wind_max_failures(i);
     }
+#endif
 }
 
 void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)

libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp

@@ -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) {

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;
 };

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
 }