From 857d799bb407ea379030fd9db77bd130fdc9d17f Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 1 Mar 2026 08:05:45 -0600 Subject: [PATCH] Add CRSF sensor input on dedicated UART Add support for receiving sensor data from CRSF-speaking devices connected to a dedicated flight controller UART, similar to MSP sensor input. Incoming CRSF frames are parsed and dispatched to INAV's real sensor subsystems so the data is automatically available to OSD, blackbox, navigation, and outgoing CRSF telemetry. Supported sensor types: - GPS (0x02): New GPS_CRSF provider feeding gpsSolDRV - Barometer (0x09): New BARO_CRSF driver with altitude-to-pressure conversion via ISA formula - Battery (0x08): New CRSF voltage/current source following the SmartPort integration pattern - Vario (0x07): Stored and used as preferred source for outgoing CRSF vario telemetry frames Configuration: serial 16777216 420000 0 0 0 set gps_provider = CRSF set baro_hardware = CRSF set vbat_meter_type = CRSF set current_meter_type = CRSF Feature is gated on MCU_FLASH_SIZE > 512 to exclude F722 and other flash-constrained targets. --- src/main/CMakeLists.txt | 7 + src/main/drivers/barometer/barometer_crsf.c | 109 +++++++ src/main/drivers/barometer/barometer_crsf.h | 29 ++ src/main/fc/fc_init.c | 5 + src/main/fc/fc_tasks.c | 21 ++ src/main/fc/settings.yaml | 8 +- src/main/io/crsf_sensor.c | 321 ++++++++++++++++++++ src/main/io/crsf_sensor.h | 35 +++ src/main/io/gps.c | 7 + src/main/io/gps.h | 1 + src/main/io/gps_crsf.c | 56 ++++ src/main/io/gps_private.h | 4 + src/main/io/serial.h | 2 +- src/main/scheduler/scheduler.h | 3 + src/main/sensors/barometer.c | 15 + src/main/sensors/barometer.h | 3 +- src/main/sensors/battery.c | 27 ++ src/main/sensors/battery_config_structs.h | 6 +- src/main/sensors/battery_sensor_crsf.c | 72 +++++ src/main/sensors/battery_sensor_crsf.h | 32 ++ src/main/target/common_post.h | 12 + src/main/telemetry/crsf.c | 13 +- 22 files changed, 779 insertions(+), 9 deletions(-) create mode 100644 src/main/drivers/barometer/barometer_crsf.c create mode 100644 src/main/drivers/barometer/barometer_crsf.h create mode 100644 src/main/io/crsf_sensor.c create mode 100644 src/main/io/crsf_sensor.h create mode 100644 src/main/io/gps_crsf.c create mode 100644 src/main/sensors/battery_sensor_crsf.c create mode 100644 src/main/sensors/battery_sensor_crsf.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..843b14977fd 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -120,6 +120,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 @@ -532,6 +534,7 @@ main_sources(COMMON_SRC io/gps_ublox.c io/gps_ublox_utils.c io/gps_msp.c + io/gps_crsf.c io/gps_fake.c io/gps_private.h io/ledstrip.c @@ -552,6 +555,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 @@ -595,6 +600,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 diff --git a/src/main/drivers/barometer/barometer_crsf.c b/src/main/drivers/barometer/barometer_crsf.c new file mode 100644 index 00000000000..8548318ba55 --- /dev/null +++ b/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 +#include + +#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 diff --git a/src/main/drivers/barometer/barometer_crsf.h b/src/main/drivers/barometer/barometer_crsf.h new file mode 100644 index 00000000000..4d03ed12898 --- /dev/null +++ b/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); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ca6c0c199..8d2e164a045 100644 --- a/src/main/fc/fc_init.c +++ b/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" @@ -298,6 +299,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 diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4ca125d5c6a..8216777ebdd 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -67,6 +67,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" @@ -295,6 +296,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) { @@ -435,6 +444,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); @@ -622,6 +634,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", diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1e1932531e5..568d7271db8 100644 --- a/src/main/fc/settings.yaml +++ b/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", "CRSF", "FAKE"] 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"] + values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT", "CRSF"] enum: currentSensor_e - name: voltage_sensor - values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT"] + values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT", "CRSF"] 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"] + values: ["UBLOX", "MSP", "CRSF", "FAKE"] enum: gpsProvider_e - name: gps_sbas_mode values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"] diff --git a/src/main/io/crsf_sensor.c b/src/main/io/crsf_sensor.c new file mode 100644 index 00000000000..f4faa70f06f --- /dev/null +++ b/src/main/io/crsf_sensor.c @@ -0,0 +1,321 @@ +/* + * 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 +#include +#include +#include + +#include "platform.h" + +#ifdef USE_CRSF_SENSOR_INPUT + +#include "build/build_config.h" + +#include "common/crc.h" +#include "common/utils.h" + +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "io/serial.h" +#include "io/crsf_sensor.h" + +#include "rx/crsf.h" + +#ifdef USE_GPS_PROTO_CRSF +#include "io/gps.h" +#include "io/gps_private.h" +#endif + +#ifdef USE_BARO_CRSF +#include "drivers/barometer/barometer_crsf.h" +#endif + +#ifdef USE_BATTERY_SENSOR_CRSF +#include "sensors/battery_sensor_crsf.h" +#endif + +#define CRSF_SENSOR_BAUDRATE 420000 +#define CRSF_SENSOR_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) +#define CRSF_SENSOR_TIME_NEEDED_PER_FRAME_US 1750 + +#define CRSF_SENSOR_VARIO_TIMEOUT_MS 2000 + +static serialPort_t *crsfSensorPort; +static uint8_t crsfSensorFrame[CRSF_FRAME_SIZE_MAX]; +static uint8_t crsfSensorFramePosition; +static timeUs_t crsfSensorFrameStartAtUs; +static volatile bool crsfSensorFrameDone; + +// Vario data storage +static int16_t crsfSensorVario; +static timeMs_t crsfSensorVarioLastUpdateMs; + +static uint8_t crsfSensorFrameCRC(void) +{ + // CRC includes type and payload (bytes 2..frameLength) + uint8_t crc = crc8_dvb_s2(0, crsfSensorFrame[2]); // type + const uint8_t frameLength = crsfSensorFrame[1]; + for (int ii = 0; ii < frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) { + crc = crc8_dvb_s2(crc, crsfSensorFrame[3 + ii]); + } + return crc; +} + +// Big-endian decode helpers matching CRSF wire format +static int32_t crsfSensorRead32(const uint8_t *p) +{ + return (int32_t)((uint32_t)p[0] << 24 | (uint32_t)p[1] << 16 | (uint32_t)p[2] << 8 | p[3]); +} + +static uint16_t crsfSensorReadU16(const uint8_t *p) +{ + return (uint16_t)(p[0] << 8 | p[1]); +} + +static int16_t crsfSensorRead16(const uint8_t *p) +{ + return (int16_t)crsfSensorReadU16(p); +} + +#ifdef USE_GPS_PROTO_CRSF +static void crsfSensorHandleGPS(const uint8_t *payload) +{ + // GPS payload: lat(4) lon(4) groundspeed(2) heading(2) altitude(2) sats(1) + int32_t lat = crsfSensorRead32(payload); + int32_t lon = crsfSensorRead32(payload + 4); + uint16_t groundSpeed = crsfSensorReadU16(payload + 8); // km/h * 10 + uint16_t heading = crsfSensorReadU16(payload + 10); // degrees * 100 + uint16_t altitude = crsfSensorReadU16(payload + 12); // meters + 1000 offset + uint8_t numSat = payload[14]; + + gpsSolDRV.llh.lat = lat; // degree / 10^7, same as INAV + gpsSolDRV.llh.lon = lon; + gpsSolDRV.llh.alt = (int32_t)(altitude - 1000) * 100; // meters to cm + gpsSolDRV.groundSpeed = (groundSpeed * 100 + 18) / 36; // km/h*10 to cm/s + gpsSolDRV.groundCourse = heading / 10; // centidegrees to decidegrees + gpsSolDRV.numSat = numSat; + + if (numSat >= 4) { + gpsSolDRV.fixType = GPS_FIX_3D; + } else if (numSat >= 2) { + gpsSolDRV.fixType = GPS_FIX_2D; + } else { + gpsSolDRV.fixType = GPS_NO_FIX; + } + + // CRSF GPS frame doesn't include NED velocity or accuracy + gpsSolDRV.flags.validVelNE = false; + gpsSolDRV.flags.validVelD = false; + gpsSolDRV.flags.validEPE = false; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.eph = gpsConstrainEPE(200); // default ~2m + gpsSolDRV.epv = gpsConstrainEPE(400); // default ~4m + gpsSolDRV.hdop = gpsConstrainHDOP(200); + + gpsProcessNewDriverData(); + crsfGPSNewDataReady(); +} +#endif + +#ifdef USE_BARO_CRSF +static void crsfSensorHandleBaro(const uint8_t *payload) +{ + // Baro payload: altitude_packed (uint16_t big-endian) + // Format matches outgoing CRSF baro encoding: + // bit 15 clear: altitude_dm = packed - 10000 (fine, dm resolution) + // bit 15 set: altitude_dm = (packed & 0x7fff) * 10 - 5 (coarse, meter resolution) + uint16_t packed = crsfSensorReadU16(payload); + int32_t altitude_dm; + + if (packed & 0x8000) { + altitude_dm = (int32_t)(packed & 0x7fff) * 10 - 5; + } else { + altitude_dm = (int32_t)packed - 10000; + } + + // Convert altitude (dm) to pressure using ISA formula: + // P = 101325 * (1 - h/44330)^5.255 + float altitude_m = altitude_dm / 10.0f; + float pressure_ratio = 1.0f - altitude_m / 44330.0f; + + // Clamp to avoid negative values from extreme altitudes + if (pressure_ratio < 0.01f) { + pressure_ratio = 0.01f; + } + + float pressure_pa = 101325.0f * powf(pressure_ratio, 5.255f); + crsfBaroReceiveNewData((int32_t)pressure_pa, 2500); // 25.00 degC default +} +#endif + +#ifdef USE_BATTERY_SENSOR_CRSF +static void crsfSensorHandleBattery(const uint8_t *payload) +{ + // Battery payload: voltage(2) current(2) capacity(3) remaining(1) + // Voltage: decivolts on wire, INAV uses 0.01V + // Current: deciamps on wire, INAV uses 0.01A + uint16_t voltage_dv = crsfSensorReadU16(payload); + uint16_t current_da = crsfSensorReadU16(payload + 2); + uint32_t capacity_mah = ((uint32_t)payload[4] << 16) | ((uint32_t)payload[5] << 8) | payload[6]; + uint8_t remaining = payload[7]; + + crsfBatterySensorReceiveNewData( + voltage_dv * 10, // decivolts to 0.01V + current_da * 10, // deciamps to 0.01A (centamps) + capacity_mah, + remaining + ); +} +#endif + +static void crsfSensorHandleVario(const uint8_t *payload) +{ + crsfSensorVario = crsfSensorRead16(payload); // cm/s + crsfSensorVarioLastUpdateMs = millis(); +} + +static void crsfSensorDispatchFrame(void) +{ + const uint8_t frameLength = crsfSensorFrame[1]; + const uint8_t fullFrameLength = frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; + const uint8_t type = crsfSensorFrame[2]; + + // CRC check + const uint8_t crc = crsfSensorFrameCRC(); + if (crc != crsfSensorFrame[fullFrameLength - 1]) { + return; + } + + const uint8_t *payload = &crsfSensorFrame[3]; + + switch (type) { +#ifdef USE_GPS_PROTO_CRSF + case CRSF_FRAMETYPE_GPS: + if (frameLength >= CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { + crsfSensorHandleGPS(payload); + } + break; +#endif + +#ifdef USE_BARO_CRSF + case CRSF_FRAMETYPE_BAROMETER_ALTITUDE: + if (frameLength >= CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { + crsfSensorHandleBaro(payload); + } + break; +#endif + +#ifdef USE_BATTERY_SENSOR_CRSF + case CRSF_FRAMETYPE_BATTERY_SENSOR: + if (frameLength >= CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { + crsfSensorHandleBattery(payload); + } + break; +#endif + + case CRSF_FRAMETYPE_VARIO_SENSOR: + if (frameLength >= CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { + crsfSensorHandleVario(payload); + } + break; + + default: + break; + } +} + +static void crsfSensorDataReceive(uint16_t c, void *rxCallbackData) +{ + UNUSED(rxCallbackData); + + const timeUs_t currentTimeUs = microsISR(); + + if (cmpTimeUs(currentTimeUs, crsfSensorFrameStartAtUs) > CRSF_SENSOR_TIME_NEEDED_PER_FRAME_US) { + crsfSensorFramePosition = 0; + } + + if (crsfSensorFramePosition == 0) { + crsfSensorFrameStartAtUs = currentTimeUs; + } + + const int fullFrameLength = crsfSensorFramePosition < 3 ? 5 : crsfSensorFrame[1] + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; + + if (fullFrameLength > CRSF_FRAME_SIZE_MAX) { + crsfSensorFramePosition = 0; + return; + } + + if (crsfSensorFramePosition < fullFrameLength) { + crsfSensorFrame[crsfSensorFramePosition++] = (uint8_t)c; + if (crsfSensorFramePosition >= fullFrameLength) { + crsfSensorFrameDone = true; + crsfSensorFramePosition = 0; + } + } +} + +void crsfSensorInputInit(void) +{ + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_CRSF_SENSOR); + if (!portConfig) { + return; + } + + crsfSensorPort = openSerialPort(portConfig->identifier, + FUNCTION_CRSF_SENSOR, + crsfSensorDataReceive, + NULL, + CRSF_SENSOR_BAUDRATE, + MODE_RX, + CRSF_SENSOR_PORT_OPTIONS + ); + + crsfSensorVario = 0; + crsfSensorVarioLastUpdateMs = 0; +} + +bool crsfSensorVarioIsValid(void) +{ + return (crsfSensorVarioLastUpdateMs > 0) && + ((millis() - crsfSensorVarioLastUpdateMs) < CRSF_SENSOR_VARIO_TIMEOUT_MS); +} + +int16_t crsfSensorGetVario(void) +{ + return crsfSensorVario; +} + +// Called from scheduler to process completed frames outside ISR context +void crsfSensorProcess(void) +{ + if (crsfSensorFrameDone) { + crsfSensorFrameDone = false; + crsfSensorDispatchFrame(); + } +} + +#endif diff --git a/src/main/io/crsf_sensor.h b/src/main/io/crsf_sensor.h new file mode 100644 index 00000000000..e7fe884601e --- /dev/null +++ b/src/main/io/crsf_sensor.h @@ -0,0 +1,35 @@ +/* + * 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 + +#include +#include + +void crsfSensorInputInit(void); +void crsfSensorProcess(void); + +// Vario data from CRSF sensor (no hardware vario concept in INAV) +bool crsfSensorVarioIsValid(void); +int16_t crsfSensorGetVario(void); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c8dc6788df3..8fd3773b37c 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -109,6 +109,13 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { { false, 0, NULL, NULL }, #endif + /* CRSF GPS */ +#ifdef USE_GPS_PROTO_CRSF + { true, 0, &gpsRestartCRSF, &gpsHandleCRSF }, +#else + { false, 0, NULL, NULL }, +#endif + #ifdef USE_GPS_FAKE {true, 0, &gpsFakeRestart, &gpsFakeHandle}, #else diff --git a/src/main/io/gps.h b/src/main/io/gps.h index c14db4a7630..c175624ab12 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -35,6 +35,7 @@ typedef enum { GPS_UBLOX = 0, GPS_MSP, + GPS_CRSF, GPS_FAKE, GPS_PROVIDER_COUNT } gpsProvider_e; diff --git a/src/main/io/gps_crsf.c b/src/main/io/gps_crsf.c new file mode 100644 index 00000000000..c4f5c27c838 --- /dev/null +++ b/src/main/io/gps_crsf.c @@ -0,0 +1,56 @@ +/* + * 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 +#include + +#include "platform.h" +#include "build/build_config.h" + +#if defined(USE_GPS_PROTO_CRSF) + +#include "io/gps.h" +#include "io/gps_private.h" + +static bool newDataReady; + +void gpsRestartCRSF(void) +{ + // NOP — data arrives via crsfSensorHandleGPS() +} + +void gpsHandleCRSF(void) +{ + if (newDataReady) { + gpsProcessNewSolutionData(false); + newDataReady = false; + } +} + +void crsfGPSNewDataReady(void) +{ + newDataReady = true; +} + +#endif diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index e5234e70248..5d4cc57ca55 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -87,6 +87,10 @@ extern void gpsHandleUBLOX(void); extern void gpsRestartMSP(void); extern void gpsHandleMSP(void); +extern void gpsRestartCRSF(void); +extern void gpsHandleCRSF(void); +extern void crsfGPSNewDataReady(void); + #if defined(USE_GPS_FAKE) extern void gpsFakeRestart(void); extern void gpsFakeHandle(void); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index a746dc77177..b62e63f0c7d 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -55,7 +55,7 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 - FUNCTION_UNUSED_2 = (1 << 24), // 16777216 + FUNCTION_CRSF_SENSOR = (1 << 24), // 16777216 FUNCTION_MSP_OSD = (1 << 25), // 33554432 FUNCTION_GIMBAL = (1 << 26), // 67108864 FUNCTION_GIMBAL_HEADTRACKER = (1 << 27), // 134217728 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5d38a6dba59..fd2be7dce80 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -119,6 +119,9 @@ typedef enum { #if defined(USE_SMARTPORT_MASTER) TASK_SMARTPORT_MASTER, #endif +#if defined(USE_CRSF_SENSOR_INPUT) + TASK_CRSF_SENSOR, +#endif #ifdef USE_IRLOCK TASK_IRLOCK, #endif diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index a9c2d8441dc..aaea9dda501 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -42,6 +42,7 @@ #include "drivers/barometer/barometer_dps310.h" #include "drivers/barometer/barometer_2smpb_02b.h" #include "drivers/barometer/barometer_msp.h" +#include "drivers/barometer/barometer_crsf.h" #include "drivers/time.h" #include "fc/runtime_config.h" @@ -210,6 +211,20 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) } FALLTHROUGH; + case BARO_CRSF: +#ifdef USE_BARO_CRSF + // Skip autodetection for CRSF baro, only allow manual config + if (baroHardwareToUse != BARO_AUTODETECT && crsfBaroDetect(dev)) { + baroHardware = BARO_CRSF; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (baroHardwareToUse != BARO_AUTODETECT) { + break; + } + FALLTHROUGH; + case BARO_FAKE: #ifdef USE_FAKE_BARO if (fakeBaroDetect(dev)) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 0e8e918d70f..56aa0023d2e 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -34,7 +34,8 @@ typedef enum { BARO_DPS310 = 9, BARO_B2SMPB = 10, BARO_MSP = 11, - BARO_FAKE = 12, + BARO_CRSF = 12, + BARO_FAKE = 13, BARO_MAX = BARO_FAKE } baroSensor_e; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 38d410610e1..008e99f7d6a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -65,6 +65,9 @@ #if defined(USE_SMARTPORT_MASTER) #include "io/smartport_master.h" #endif +#if defined(USE_BATTERY_SENSOR_CRSF) +#include "sensors/battery_sensor_crsf.h" +#endif #define ADCVREF 3300 // in mV (3300 = 3.3V) @@ -304,6 +307,18 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) vbat = 0; } break; +#endif +#if defined(USE_BATTERY_SENSOR_CRSF) + case VOLTAGE_SENSOR_CRSF: + { + int16_t *crsfVoltageData = crsfBatterySensorGetVoltageData(); + if (crsfVoltageData) { + vbat = *crsfVoltageData; + } else { + vbat = 0; + } + } + break; #endif case VOLTAGE_SENSOR_NONE: default: @@ -622,6 +637,18 @@ void currentMeterUpdate(timeUs_t timeDelta) } break; #endif +#if defined(USE_BATTERY_SENSOR_CRSF) + case CURRENT_SENSOR_CRSF: + { + int16_t *crsfCurrentData = crsfBatterySensorGetCurrentData(); + if (crsfCurrentData) { + amperage = *crsfCurrentData; + } else { + amperage = 0; + } + } + break; +#endif #if defined(USE_FAKE_BATT_SENSOR) case CURRENT_SENSOR_FAKE: amperage = fakeBattSensorGetAmerperage(); diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 7c4157cb4c0..05edb3eaff0 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -32,7 +32,8 @@ typedef enum { CURRENT_SENSOR_FAKE, CURRENT_SENSOR_ESC, CURRENT_SENSOR_SMARTPORT, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_SMARTPORT + CURRENT_SENSOR_CRSF, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_CRSF } currentSensor_e; typedef enum { @@ -41,7 +42,8 @@ typedef enum { VOLTAGE_SENSOR_ESC, VOLTAGE_SENSOR_FAKE, VOLTAGE_SENSOR_SMARTPORT, - VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_SMARTPORT + VOLTAGE_SENSOR_CRSF, + VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_CRSF } voltageSensor_e; typedef enum { diff --git a/src/main/sensors/battery_sensor_crsf.c b/src/main/sensors/battery_sensor_crsf.c new file mode 100644 index 00000000000..22ff7b343e1 --- /dev/null +++ b/src/main/sensors/battery_sensor_crsf.c @@ -0,0 +1,72 @@ +/* + * 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 +#include +#include +#include + +#include "platform.h" + +#if defined(USE_BATTERY_SENSOR_CRSF) + +#include "common/maths.h" +#include "common/utils.h" +#include "drivers/time.h" +#include "sensors/battery_sensor_crsf.h" + +#define CRSF_BATTERY_TIMEOUT_MS 2000 + +static int16_t crsfVoltage; // 0.01V units +static int16_t crsfCurrent; // 0.01A units (centamps) +static uint32_t crsfCapacity; // mAh +static uint8_t crsfRemaining; // percent +static timeMs_t crsfBatteryLastUpdateMs; + +void crsfBatterySensorReceiveNewData(uint16_t voltage, uint16_t current, uint32_t capacity, uint8_t remaining) +{ + crsfVoltage = MIN(voltage, INT16_MAX); + crsfCurrent = MIN(current, INT16_MAX); + crsfCapacity = capacity; + crsfRemaining = remaining; + crsfBatteryLastUpdateMs = millis(); +} + +int16_t *crsfBatterySensorGetVoltageData(void) +{ + if (crsfBatteryLastUpdateMs == 0 || (millis() - crsfBatteryLastUpdateMs) > CRSF_BATTERY_TIMEOUT_MS) { + return NULL; + } + return &crsfVoltage; +} + +int16_t *crsfBatterySensorGetCurrentData(void) +{ + if (crsfBatteryLastUpdateMs == 0 || (millis() - crsfBatteryLastUpdateMs) > CRSF_BATTERY_TIMEOUT_MS) { + return NULL; + } + return &crsfCurrent; +} + +#endif diff --git a/src/main/sensors/battery_sensor_crsf.h b/src/main/sensors/battery_sensor_crsf.h new file mode 100644 index 00000000000..1f8d4f11c86 --- /dev/null +++ b/src/main/sensors/battery_sensor_crsf.h @@ -0,0 +1,32 @@ +/* + * 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 + +#include + +void crsfBatterySensorReceiveNewData(uint16_t voltage, uint16_t current, uint32_t capacity, uint8_t remaining); + +int16_t *crsfBatterySensorGetVoltageData(void); +int16_t *crsfBatterySensorGetCurrentData(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ccd14d35a4a..7517e3ff1ba 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -131,6 +131,18 @@ extern uint8_t __config_end; #endif +// CRSF sensor input on a dedicated UART — excluded from flash-constrained targets +#if defined(USE_SERIALRX_CRSF) && (!defined(MCU_FLASH_SIZE) || MCU_FLASH_SIZE > 512) +#define USE_CRSF_SENSOR_INPUT +#define USE_BATTERY_SENSOR_CRSF +#if defined(USE_GPS) +#define USE_GPS_PROTO_CRSF +#endif +#if defined(USE_BARO) +#define USE_BARO_CRSF +#endif +#endif + #ifdef USE_ESC_SENSOR #define USE_RPM_FILTER #endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d86822ada7a..17187976378 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -62,6 +62,8 @@ #include "telemetry/telemetry.h" #include "telemetry/msp_shared.h" +#include "io/crsf_sensor.h" + #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz #define CRSF_DEVICEINFO_VERSION 0x01 @@ -240,7 +242,16 @@ static void crsfFrameVarioSensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR); - crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z))); + int16_t vario; +#ifdef USE_CRSF_SENSOR_INPUT + if (crsfSensorVarioIsValid()) { + vario = crsfSensorGetVario(); + } else +#endif + { + vario = lrintf(getEstimatedActualVelocity(Z)); + } + crsfSerialize16(dst, vario); } /*