Merge pull request #11379 from sensei-hacker/feature-crsf-sensor-input

Add CRSF sensor input on dedicated UART

Author: sensei-hacker

docs/Settings.md

@@ -514,6 +514,7 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det
 | B2SMPB |  |
 | MSP |  |
 | FAKE |  |
+| CRSF |  |
 
 ---
 
@@ -695,6 +696,7 @@ ADC, VIRTUAL, FAKE, ESC, SMARTPORT, CAN, NONE. The virtual current sensor, once
 | FAKE |  |
 | ESC |  |
 | SMARTPORT |  |
+| CRSF |  |
 | CAN |  |
 
 ---
@@ -1863,6 +1865,7 @@ Which GPS protocol to be used.
 | --- | --- |
 | UBLOX | Default |
 | MSP |  |
+| CRSF |  |
 | FAKE |  |
 | DRONECAN |  |
 
@@ -6937,6 +6940,7 @@ Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`, 'CAN'.
 | ESC |  |
 | FAKE |  |
 | SMARTPORT |  |
+| CRSF |  |
 | CAN |  |
 
 ---

src/main/CMakeLists.txt

@@ -122,6 +122,8 @@ main_sources(COMMON_SRC
     drivers/barometer/barometer_spl06.h
     drivers/barometer/barometer_msp.c
     drivers/barometer/barometer_msp.h
+    drivers/barometer/barometer_crsf.c
+    drivers/barometer/barometer_crsf.h
     drivers/barometer/barometer_2smpb_02b.c
     drivers/barometer/barometer_2smpb_02b.h
 
@@ -543,6 +545,7 @@ main_sources(COMMON_SRC
     io/gps_ublox.c
     io/gps_ublox_utils.c
     io/gps_msp.c
+    io/gps_crsf.c
     io/gps_dronecan.c
     io/gps_fake.c
     io/gps_private.h
@@ -564,6 +567,8 @@ main_sources(COMMON_SRC
     io/osd_joystick.h
     io/smartport_master.c
     io/smartport_master.h
+    io/crsf_sensor.c
+    io/crsf_sensor.h
     io/vtx.c
     io/vtx.h
     io/vtx_string.c
@@ -607,6 +612,8 @@ main_sources(COMMON_SRC
     sensors/opflow.h
     sensors/battery_sensor_fake.c
     sensors/battery_sensor_fake.h
+    sensors/battery_sensor_crsf.c
+    sensors/battery_sensor_crsf.h
 
     telemetry/crsf.c
     telemetry/crsf.h

src/main/build/atomic.h

@@ -44,10 +44,10 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
 // Full memory barrier is placed at start and exit of block
-#ifdef UNIT_TEST
+#if defined(UNIT_TEST) || defined(SITL_BUILD)
 #define ATOMIC_BLOCK(prio) {}
 #else
 #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
                                      __ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
 
-#endif // UNIT_TEST
+#endif // UNIT_TEST || SITL_BUILD

src/main/drivers/barometer/barometer_crsf.c

@@ -0,0 +1,109 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#if defined(USE_BARO_CRSF)
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/time.h"
+#include "drivers/barometer/barometer.h"
+#include "drivers/barometer/barometer_crsf.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "fc/runtime_config.h"
+
+#define CRSF_BARO_TIMEOUT_MS    250
+
+static int32_t crsfBaroPressure;
+static int32_t crsfBaroTemperature;
+static timeMs_t crsfBaroLastUpdateMs;
+static bool crsfBaroStarted = false;
+
+static bool crsfBaroStartGet(baroDev_t *baro)
+{
+    UNUSED(baro);
+    return true;
+}
+
+static bool crsfBaroCalculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
+{
+    UNUSED(baro);
+
+    if ((millis() - crsfBaroLastUpdateMs) > CRSF_BARO_TIMEOUT_MS) {
+        sensorsClear(SENSOR_BARO);
+        crsfBaroStarted = false;
+        return false;
+    }
+
+    if (pressure)
+        *pressure = crsfBaroPressure;
+
+    if (temperature)
+        *temperature = crsfBaroTemperature;
+
+    return true;
+}
+
+void crsfBaroReceiveNewData(int32_t pressurePa, int32_t temperature)
+{
+    crsfBaroPressure = pressurePa;
+    crsfBaroTemperature = temperature;
+    crsfBaroLastUpdateMs = millis();
+
+    if (crsfBaroStarted == false && !ARMING_FLAG(WAS_EVER_ARMED)) {
+        baroStartCalibration();
+        crsfBaroStarted = true;
+        sensorsSet(SENSOR_BARO);
+    }
+}
+
+bool crsfBaroDetect(baroDev_t *baro)
+{
+    crsfBaroPressure = 101325;
+    crsfBaroTemperature = 2500;
+    crsfBaroLastUpdateMs = 0;
+
+    baro->ut_delay = 10000;
+    baro->get_ut = crsfBaroStartGet;
+    baro->start_ut = crsfBaroStartGet;
+
+    baro->up_delay = 10000;
+    baro->start_up = crsfBaroStartGet;
+    baro->get_up = crsfBaroStartGet;
+
+    baro->calculate = crsfBaroCalculate;
+
+    return true;
+}
+
+#endif

src/main/drivers/barometer/barometer_crsf.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+struct baroDev_s;
+bool crsfBaroDetect(struct baroDev_s *baro);
+void crsfBaroReceiveNewData(int32_t pressurePa, int32_t temperature);

src/main/fc/fc_init.c

@@ -120,6 +120,7 @@
 #include "io/serial.h"
 #include "io/displayport_msp.h"
 #include "io/smartport_master.h"
+#include "io/crsf_sensor.h"
 #include "io/vtx.h"
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
@@ -301,6 +302,10 @@ void init(void)
     smartportMasterInit();
 #endif
 
+#if defined(USE_CRSF_SENSOR_INPUT)
+    crsfSensorInputInit();
+#endif
+
 #if defined(USE_LOG)
     // LOG might use serial output, so we only can init it after serial port is ready
     // From this point on we can use LOG_*() to produce real-time debugging information

src/main/fc/fc_tasks.c

@@ -68,6 +68,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/osd_joystick.h"
 #include "io/smartport_master.h"
+#include "io/crsf_sensor.h"
 #include "io/vtx.h"
 #include "io/vtx_msp.h"
 #include "io/osd_dji_hd.h"
@@ -296,6 +297,14 @@ void taskSmartportMaster(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_CRSF_SENSOR_INPUT)
+void taskCrsfSensor(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    crsfSensorProcess();
+}
+#endif
+
 #ifdef USE_LED_STRIP
 void taskLedStrip(timeUs_t currentTimeUs)
 {
@@ -443,6 +452,9 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+#if defined(USE_CRSF_SENSOR_INPUT)
+    setTaskEnabled(TASK_CRSF_SENSOR, true);
+#endif
 
 #ifdef USE_SERIAL_GIMBAL
     setTaskEnabled(TASK_GIMBAL, true);
@@ -634,6 +646,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#if defined(USE_CRSF_SENSOR_INPUT)
+    [TASK_CRSF_SENSOR] = {
+        .taskName = "CRSF SENSOR",
+        .taskFunc = taskCrsfSensor,
+        .desiredPeriod = TASK_PERIOD_HZ(100),         // 100 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #ifdef USE_LED_STRIP
     [TASK_LEDSTRIP] = {
         .taskName = "LEDSTRIP",

src/main/fc/settings.yaml

@@ -14,7 +14,7 @@ tables:
     values: ["NONE", "CXOF", "MSP", "FAKE"]
     enum: opticalFlowSensor_e
   - name: baro_hardware
-    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
+    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE", "CRSF"]
     enum: baroSensor_e
   - name: pitot_hardware
     values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
@@ -33,16 +33,16 @@ tables:
   - name: failsafe_procedure
     values: ["LAND", "DROP", "RTH", "NONE"]
   - name: current_sensor
-    values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT", "CAN"]
+    values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT", "CRSF", "CAN"]
     enum: currentSensor_e
   - name: voltage_sensor
-    values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT", "CAN"]
+    values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT", "CRSF", "CAN"]
     enum: voltageSensor_e
   - name: imu_inertia_comp_method
     values: ["VELNED", "TURNRATE","ADAPTIVE"]
     enum: imu_inertia_comp_method_e
   - name: gps_provider
-    values: ["UBLOX", "MSP", "FAKE", "DRONECAN"]
+    values: ["UBLOX", "MSP", "CRSF", "FAKE", "DRONECAN"]
     enum: gpsProvider_e
   - name: gps_sbas_mode
     values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]