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); } /*