diff --git a/docs/SBUS2_Telemetry.md b/docs/SBUS2_Telemetry.md new file mode 100644 index 00000000000..1a687e99b0b --- /dev/null +++ b/docs/SBUS2_Telemetry.md @@ -0,0 +1,25 @@ +# Futaba SBUS2 Telemetry + +Basic experimental support for SBUS2 telemetry has been added to INAV 8.0.0. Currently it is limited to F7 and H7 mcus only. The main reason it is limited to those MCUs is due to the requirement for an inverted UART signal and the SBUS pads in F405 usually are not bi-directional. + +The basic sensors have been tested with a Futaba T16IZ running software version 6.0E. + +An alternative to using INAV's SBUS2 support is to use SBS-01ML MAVlink Telemetry Drone Sensor instead. (not tested and not supported with older futaba radios, including my 16IZ). + +# Wiring +The SBUS2 signal should be connected to the TX PIN, not the RX PIN, like on a traditional SBUS setup. + +# Sensor mapping + +The following fixed sensor mapping is used: + +| Slot | Sensort Type | Info | +| --- | --- | --- | +| 1 | Voltage | Pack voltage and cell voltage | +| 3 | Current | Capacity = used mAh | +| 6 | rpm sensor | motor rpm. Need to set geat ratio to 1.0 | +| 7 | Temperature | ESC Temperature | +| 8 | GPS | | +| 16 | Temperature | IMU Temperature | +| 17 | Temperature | Baro Temperature | +| 18-25 | Temperature | Temperature sensor 0-7 | diff --git a/docs/Serial Gimbal.md b/docs/Serial Gimbal.md index 984e00f29d1..c2c35b90af1 100644 --- a/docs/Serial Gimbal.md +++ b/docs/Serial Gimbal.md @@ -17,7 +17,7 @@ In head tracker mode, the Serial Gimbal will ignore the axis rc channel inputs a # Gimbal Modes ## No Gimbal mode selected -Like ACRO is the default mode for flight modes, the Gimbal will default to ```FPV Mode``` or ```Follow Mode``` when no mode is selected. The gimbal will try to stablized the footag and will follow the aircraft pitch, roll and yaw movements and use user inputs to point the camera where the user wants. +Like ACRO is the default mode for flight modes, the Gimbal will default to ```FPV Mode``` or ```Follow Mode``` when no mode is selected. The gimbal will try to stablized the footage and will follow the aircraft pitch, roll and yaw movements and use user inputs to point the camera where the user wants. ## Gimbal Center This locks the gimbal camera to the center position and ignores any user input. Useful to reset the camera if you loose orientation. @@ -49,7 +49,7 @@ Allowed range: -500 - 500 ``` ## Gimbal and Headtracker on a single uart -As INAV does not process any inputs from the Walksnail Gimbal, it is possible to share the uard with the Walksnail Headtracking output by connect the fc TX to the gimbal and RX to receive the headtracker input. +As INAV does not process any inputs from the Walksnail Gimbal, it is possible to share the uart with the Walksnail Headtracking output by connect the fc TX to the gimbal and RX to receive the headtracker input. ``` gimbal_serial_single_uart = OFF Allowed values: OFF, ON diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 996630e66fa..a33aa4ee527 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -390,3 +390,21 @@ In configurator set IBUS telemetry and RX on this same port, enable telemetry fe Warning: Schematic above work also for connect telemetry only, but not work for connect rx only - will stop FC. + + +## Futaba SBUS2 telemetry + +SBUS2 telemetry requires a single connection from the TX pin of a bidirectional serial port to the SBUS2 pin on a Futaba T-FHSS or FASSTest telemetry receiver. (tested T16IZ radio and R7108SB and R3204SB receivers) + +It shares 1 line for both TX and RX, the rx pin cannot be used for other serial port stuff. +It runs at a fixed baud rate of 100000, so it needs a hardware uart capable of inverted signals. It is not available on F4 mcus. + +``` + _______ + / \ /-------------\ + | STM32 |-->UART TX-->[Bi-directional @ 100000 baud]-->| Futaba RX | + | uC |- UART RX--x[not connected] | SBUS2 port | + \_______/ \-------------/ +``` + +For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_telemetry.md) \ No newline at end of file diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 0f4049f51c5..b6ff0e10985 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -612,6 +612,10 @@ main_sources(COMMON_SRC telemetry/mavlink.h telemetry/msp_shared.c telemetry/msp_shared.h + telemetry/sbus2.c + telemetry/sbus2.h + telemetry/sbus2_sensors.c + telemetry/sbus2_sensors.h telemetry/smartport.c telemetry/smartport.h telemetry/sim.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 0d897f77101..0bb74bac1ac 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -78,6 +78,7 @@ typedef enum { DEBUG_HEADTRACKING, DEBUG_GPS, DEBUG_LULU, + DEBUG_SBUS2, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index b8d3803613f..2dbb53c0a1d 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -73,6 +73,10 @@ #define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) #define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) +#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f) +#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f) +#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f) + #define C_TO_KELVIN(temp) (temp + 273.15f) // Standard Sea Level values diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 580859fec0c..0ea43fd0ce7 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -211,7 +211,9 @@ static const char *debugModeNames[DEBUG_COUNT] = { "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", - "GPS" + "GPS", + "LULU", + "SBUS2" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index da40f9149f5..f7603eee692 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -880,7 +880,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; - if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) { + bool fwLaunchIsActive = STATE(AIRPLANE) && isNavLaunchEnabled() && armTime == 0; + + if (ARMING_FLAG(ARMED) && (!STATE(AIRPLANE) || !fwLaunchIsActive || fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED)) { flightTime += cycleTime; armTime += cycleTime; updateAccExtremes(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 6918b9a6dab..107a0e732b6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -93,6 +93,7 @@ #include "sensors/opflow.h" #include "telemetry/telemetry.h" +#include "telemetry/sbus2.h" #include "config/feature.h" @@ -437,6 +438,10 @@ void fcTasksInit(void) setTaskEnabled(TASK_HEADTRACKER, true); #endif +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) + setTaskEnabled(TASK_TELEMETRY_SBUS2,rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2); +#endif + #ifdef USE_ADAPTIVE_FILTER setTaskEnabled(TASK_ADAPTIVE_FILTER, ( gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && @@ -726,4 +731,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) + [TASK_TELEMETRY_SBUS2] = { + .taskName = "SBUS2 TLM", + .taskFunc = taskSendSbus2Telemetry, + .desiredPeriod = TASK_PERIOD_US(125), // 8kHz 2ms dead time + 650us window / sensor. + .staticPriority = TASK_PRIORITY_LOW, // timing is critical. Ideally, should be a timer interrupt triggered by sbus packet + }, +#endif + }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c65d0ffa768..28279aff1e0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -23,7 +23,7 @@ tables: values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS", "SBUS2"] - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD", "FILE"] - name: motor_pwm_protocol @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6e43d2394cd..594db21417c 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -513,7 +513,7 @@ static int logicConditionCompute( void logicConditionProcess(uint8_t i) { - const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); + const int32_t activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); if (logicConditions(i)->enabled && activatorValue && !cliMode) { @@ -522,9 +522,9 @@ void logicConditionProcess(uint8_t i) { * Latched LCs can only go from OFF to ON, not the other way */ if (!(logicConditionStates[i].flags & LOGIC_CONDITION_FLAG_LATCH)) { - const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); - const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); - const int newValue = logicConditionCompute( + const int32_t operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); + const int32_t operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); + const int32_t newValue = logicConditionCompute( logicConditionStates[i].value, logicConditions(i)->operation, operandAValue, @@ -650,15 +650,15 @@ static int logicConditionGetFlightOperandValue(int operand) { switch (operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s - return constrain((uint32_t)getFlightTime(), 0, INT16_MAX); + return constrain((uint32_t)getFlightTime(), 0, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m - return constrain(GPS_distanceToHome, 0, INT16_MAX); + return constrain(GPS_distanceToHome, 0, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m - return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX); + return constrain(getTotalTravelDistance() / 100, 0, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: @@ -713,18 +713,18 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s #ifdef USE_PITOT - return constrain(getAirspeedEstimate(), 0, INT16_MAX); + return constrain(getAirspeedEstimate(), 0, INT32_MAX); #else return false; #endif break; case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm - return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX); + return constrain(getEstimatedActualPosition(Z), INT32_MIN, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s - return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX); + return constrain(getEstimatedActualVelocity(Z), INT32_MIN, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // % @@ -793,7 +793,7 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m - return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX); + return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: @@ -938,8 +938,8 @@ static int logicConditionGetFlightModeOperandValue(int operand) { } } -int logicConditionGetOperandValue(logicOperandType_e type, int operand) { - int retVal = 0; +int32_t logicConditionGetOperandValue(logicOperandType_e type, int operand) { + int32_t retVal = 0; switch (type) { @@ -994,7 +994,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { /* * conditionId == -1 is always evaluated as true */ -int logicConditionGetValue(int8_t conditionId) { +int32_t logicConditionGetValue(int8_t conditionId) { if (conditionId >= 0) { return logicConditionStates[conditionId].value; } else { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 25d6a5a9e1f..74a7765be40 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -249,9 +249,9 @@ extern uint64_t logicConditionsGlobalFlags; void logicConditionProcess(uint8_t i); -int logicConditionGetOperandValue(logicOperandType_e type, int operand); +int32_t logicConditionGetOperandValue(logicOperandType_e type, int operand); -int logicConditionGetValue(int8_t conditionId); +int32_t logicConditionGetValue(int8_t conditionId); void logicConditionUpdateTask(timeUs_t currentTimeUs); void logicConditionReset(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 881b1128c47..f941eaebe20 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -196,6 +196,7 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig break; #endif #ifdef USE_SERIALRX_SBUS + case SERIALRX_SBUS2: case SERIALRX_SBUS: enabled = sbusInit(rxConfig, rxRuntimeConfig); break; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1aee04ec083..9e4816ec64c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -81,6 +81,7 @@ typedef enum { SERIALRX_GHST, SERIALRX_MAVLINK, SERIALRX_FBUS, + SERIALRX_SBUS2, } rxSerialReceiverType_e; #ifdef USE_24CHANNELS diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 98570387654..9c0fa30bd11 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -66,6 +66,10 @@ typedef struct sbusFrameData_s { timeUs_t lastActivityTimeUs; } sbusFrameData_t; +static uint8_t sbus2ActiveTelemetryPage = 0; +static uint8_t sbus2ActiveTelemetrySlot = 0; +timeUs_t frameTime = 0; + // Receive ISR callback static void sbusDataReceive(uint16_t c, void *data) { @@ -98,10 +102,20 @@ static void sbusDataReceive(uint16_t c, void *data) // Do some sanity check switch (frame->endByte) { case 0x00: // This is S.BUS 1 - case 0x04: // S.BUS 2 receiver voltage - case 0x14: // S.BUS 2 GPS/baro - case 0x24: // Unknown SBUS2 data - case 0x34: // Unknown SBUS2 data + case 0x04: // S.BUS 2 telemetry page 1 + case 0x14: // S.BUS 2 telemetry page 2 + case 0x24: // S.BUS 2 telemetry page 3 + case 0x34: // S.BUS 2 telemetry page 4 + if(frame->endByte & 0x4) { + sbus2ActiveTelemetryPage = (frame->endByte >> 4) & 0xF; + frameTime = currentTimeUs; + } else { + sbus2ActiveTelemetryPage = 0; + sbus2ActiveTelemetrySlot = 0; + frameTime = -1; + } + + frameValid = true; sbusFrameData->state = STATE_SBUS_WAIT_SYNC; break; @@ -141,6 +155,8 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) // Reset the frameDone flag - tell ISR that we're ready to receive next frame sbusFrameData->frameDone = false; + //taskSendSbus2Telemetry(micros()); + // Calculate "virtual link quality based on packet loss metric" if (retValue & RX_FRAME_COMPLETE) { lqTrackerAccumulate(rxRuntimeConfig->lqTracker, ((retValue & RX_FRAME_DROPPED) || (retValue & RX_FRAME_FAILSAFE)) ? 0 : RSSI_MAX_VALUE); @@ -179,14 +195,15 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC sbusDataReceive, &sbusFrameData, sbusBaudRate, - portShared ? MODE_RXTX : MODE_RX, + (portShared || rxConfig->serialrx_provider == SERIALRX_SBUS2) ? MODE_RXTX : MODE_RX, SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | + ((rxConfig->serialrx_provider == SERIALRX_SBUS2) ? SERIAL_BIDIR : 0) | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) ); #ifdef USE_TELEMETRY - if (portShared) { + if (portShared || (rxConfig->serialrx_provider == SERIALRX_SBUS2)) { telemetrySharedPort = sBusPort; } #endif @@ -203,4 +220,22 @@ bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig { return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST); } -#endif + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) +timeUs_t sbusGetLastFrameTime(void) { + return frameTime; +} + +uint8_t sbusGetCurrentTelemetryNextSlot(void) +{ + uint8_t current = sbus2ActiveTelemetrySlot; + sbus2ActiveTelemetrySlot++; + return current; +} + +uint8_t sbusGetCurrentTelemetryPage(void) { + return sbus2ActiveTelemetryPage; +} +#endif // USE_TELEMETRY && USE_SBUS2_TELEMETRY + +#endif // USE_SERIAL_RX diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index 1fcd3680d0e..aa550b618e1 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -19,5 +19,13 @@ #define SBUS_DEFAULT_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case +#include "rx/rx.h" + bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); + +#ifdef USE_TELEMETRY_SBUS2 +uint8_t sbusGetCurrentTelemetryPage(void); +uint8_t sbusGetCurrentTelemetryNextSlot(void); +timeUs_t sbusGetLastFrameTime(void); +#endif diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index a43206840fe..176812d8f3f 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -133,6 +133,10 @@ typedef enum { TASK_HEADTRACKER, #endif +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) + TASK_TELEMETRY_SBUS2, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/DAKEFPVF405/target.h b/src/main/target/DAKEFPVF405/target.h index 41c128668ef..9b1aef0721a 100644 --- a/src/main/target/DAKEFPVF405/target.h +++ b/src/main/target/DAKEFPVF405/target.h @@ -67,6 +67,11 @@ #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN PA4 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 + //Baro #define USE_BARO #define USE_BARO_BMP280 diff --git a/src/main/target/DAKEFPVF722/target.h b/src/main/target/DAKEFPVF722/target.h index 92cc65783b8..f0f4584e801 100644 --- a/src/main/target/DAKEFPVF722/target.h +++ b/src/main/target/DAKEFPVF722/target.h @@ -57,6 +57,21 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_BUS BUS_SPI1 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 + //Baro #define USE_BARO #define USE_BARO_BMP280 diff --git a/src/main/target/KAKUTEF4WING/CMakeLists.txt b/src/main/target/KAKUTEF4WING/CMakeLists.txt new file mode 100644 index 00000000000..bf96ecb5d3b --- /dev/null +++ b/src/main/target/KAKUTEF4WING/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(KAKUTEF4WING) diff --git a/src/main/target/KAKUTEF4WING/config.c b/src/main/target/KAKUTEF4WING/config.c new file mode 100755 index 00000000000..206221e9930 --- /dev/null +++ b/src/main/target/KAKUTEF4WING/config.c @@ -0,0 +1,42 @@ +/* + * This file is part of INAV. + * + * 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 "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "rx/rx.h" + +#include "io/piniobox.h" +#include "drivers/serial.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} + diff --git a/src/main/target/KAKUTEF4WING/hardware_setup.c b/src/main/target/KAKUTEF4WING/hardware_setup.c new file mode 100644 index 00000000000..d2045cc0f01 --- /dev/null +++ b/src/main/target/KAKUTEF4WING/hardware_setup.c @@ -0,0 +1,42 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" + +void initialisePreBootHardware(void) +{ + // User1 + IOInit(DEFIO_IO(PB14), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PB14), IOCFG_OUT_PP); + IOLo(DEFIO_IO(PB14)); + + // User2 + IOInit(DEFIO_IO(PB15), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); + IOConfigGPIO(DEFIO_IO(PB15), IOCFG_OUT_PP); + IOLo(DEFIO_IO(PB15)); +} diff --git a/src/main/target/KAKUTEF4WING/target.c b/src/main/target/KAKUTEF4WING/target.c new file mode 100755 index 00000000000..f6956dae169 --- /dev/null +++ b/src/main/target/KAKUTEF4WING/target.c @@ -0,0 +1,42 @@ +/* + * This file is part of INAV. + * + * 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 "drivers/io.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT - DMA2_ST1_CH6 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2_OUT - DMA2_ST2_CH6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT - DMA1_ST7_CH5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST2_CH5 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5_OUT - DMA2_ST4_CH7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT - DMA2_ST7_CH7 + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED_STRIP-DMA1_ST6_CH3 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEF4WING/target.h b/src/main/target/KAKUTEF4WING/target.h new file mode 100644 index 00000000000..70eb125646a --- /dev/null +++ b/src/main/target/KAKUTEF4WING/target.h @@ -0,0 +1,183 @@ +/* + * This file is part of INAV. + * + * 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 + +#define TARGET_BOARD_IDENTIFIER "F4WI" +#define USBD_PRODUCT_STRING "KakuteF4Wing" + +#define USE_TARGET_CONFIG + +// ******* Status LED***************** +#define LED0 PC5 + +// ******* Internal IMU ICM42688****** +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 +#define ICM42605_EXTI_PIN PB12 + +// *************** I2C **************** +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +// ********** External MAG On I2C2****** +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL + +// ********** External Devices On I2C2****** +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// ********** Internal BARO On I2C2********* +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +// *************** AT7456 OSD *************** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PC15 + +// *************** SPI FLASH BLACKBOX********* +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PC14 +#define M25P16_SPI_BUS BUS_SPI3 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** USB VCP ******************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PA10 +#define VBUS_SENSING_ENABLED + +// *************** UART ******************** +#define USE_UART_INVERTER +//UART1 +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +//UART2 +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +//The 4V5 pads close to UART3 are powered by both BEC and USB +//Config UART3 to serialRX, So Receiver is powered when USB Plug-IN. +//UART3: SerialRX by Default +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 +#define INVERTER_PIN_UART3_RX PC13 + +//UART5 +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +//UART6 +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +// # define USE_SOFTSERIAL1 +# define SERIAL_PORT_COUNT 6 + +// *************** SPI ******************** +#define USE_SPI +// SPI1: Connected to ICM gyro +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +// SPI2: Connected to OSD +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PC15 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +// SPI3: Connected to flash memory +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PC14 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +// *************** Battery Voltage Sense*********** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_SCALE 250 // Current_Meter 1V=40A +#define VBAT_SCALE_DEFAULT 1100 // VBAT_ADC 1V=11V + +// *************** LED_STRIP ********************** +#define USE_LED_STRIP +#define WS2811_PIN PA1 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST6_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream6 +#define WS2811_DMA_CHANNEL DMA_Channel_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB14 // USER1 +#define PINIO2_PIN PB15 // USER2 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_GPS) + +// ***********Set rx type and procotol*************** +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 6 + + diff --git a/src/main/target/MICOAIR405MINI/CMakeLists.txt b/src/main/target/MICOAIR405MINI/CMakeLists.txt new file mode 100644 index 00000000000..27ed5c945f4 --- /dev/null +++ b/src/main/target/MICOAIR405MINI/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MICOAIR405MINI) \ No newline at end of file diff --git a/src/main/target/MICOAIR405MINI/config.c b/src/main/target/MICOAIR405MINI/config.c new file mode 100644 index 00000000000..00ee10a378d --- /dev/null +++ b/src/main/target/MICOAIR405MINI/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" +#include "fc/config.h" +#include "sensors/gyro.h" + + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_MSP_OSD; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_ESCSERIAL; +} diff --git a/src/main/target/MICOAIR405MINI/target.c b/src/main/target/MICOAIR405MINI/target.c new file mode 100644 index 00000000000..6903cb1cc49 --- /dev/null +++ b/src/main/target/MICOAIR405MINI/target.c @@ -0,0 +1,36 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MICOAIR405MINI/target.h b/src/main/target/MICOAIR405MINI/target.h new file mode 100644 index 00000000000..4b7e0b811da --- /dev/null +++ b/src/main/target/MICOAIR405MINI/target.h @@ -0,0 +1,142 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "MINI" + +#define USBD_PRODUCT_STRING "MICOAIR405MINI" + +// *************** LED ********************** +#define LED0 PC5 +#define LED1 PC4 +#define LED2 PA8 + +// *************** SPI: Gyro & ACC & BARO & OSD & SDCARD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_SPI_BUS BUS_SPI2 +#define BMI270_CS_PIN PC14 + +#define USE_BARO +#define USE_BARO_DPS310 +#define DPS310_SPI_BUS BUS_SPI2 +#define DPS310_CS_PIN PC13 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +#define USE_BLACKBOX +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC9 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PC15 + +#define SERIAL_PORT_COUNT 7 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 +#define USE_UART_INVERTER + +// *************** I2C **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +// *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 2121 +#define CURRENT_METER_SCALE 402 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 9 diff --git a/src/main/target/MICOAIR405V2/CMakeLists.txt b/src/main/target/MICOAIR405V2/CMakeLists.txt new file mode 100644 index 00000000000..1cd8dfbe968 --- /dev/null +++ b/src/main/target/MICOAIR405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MICOAIR405V2) diff --git a/src/main/target/MICOAIR405V2/config.c b/src/main/target/MICOAIR405V2/config.c new file mode 100644 index 00000000000..00ee10a378d --- /dev/null +++ b/src/main/target/MICOAIR405V2/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" +#include "fc/config.h" +#include "sensors/gyro.h" + + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_MSP_OSD; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_ESCSERIAL; +} diff --git a/src/main/target/MICOAIR405V2/target.c b/src/main/target/MICOAIR405V2/target.c new file mode 100644 index 00000000000..8c93f5394af --- /dev/null +++ b/src/main/target/MICOAIR405V2/target.c @@ -0,0 +1,37 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,2) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 -D(1,5) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,6) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,4) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 -D(1,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MICOAIR405V2/target.h b/src/main/target/MICOAIR405V2/target.h new file mode 100644 index 00000000000..c4a54f003b6 --- /dev/null +++ b/src/main/target/MICOAIR405V2/target.h @@ -0,0 +1,143 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "M405" + +#define USBD_PRODUCT_STRING "MICOAIR405V2" + +// *************** LED ********************** +#define LED0 PC5 +#define LED1 PC4 +#define LED2 PA8 + +// *************** SPI: Gyro & ACC & BARO & OSD & SDCARD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_IMU_BMI088 +#define IMU_BMI088_ALIGN CW270_DEG +#define BMI088_SPI_BUS BUS_SPI2 +#define BMI088_GYRO_CS_PIN PC14 +#define BMI088_ACC_CS_PIN PC13 + +#define USE_BARO +#define USE_BARO_SPL06 +#define SPL06_SPI_BUS BUS_SPI2 +#define SPL06_CS_PIN PA4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +#define USE_BLACKBOX +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC9 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PC15 + +#define SERIAL_PORT_COUNT 7 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 +#define USE_UART_INVERTER + +// *************** I2C **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +// *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 2121 +#define CURRENT_METER_SCALE 402 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/MICOAIR743/CMakeLists.txt b/src/main/target/MICOAIR743/CMakeLists.txt new file mode 100644 index 00000000000..1d381638780 --- /dev/null +++ b/src/main/target/MICOAIR743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(MICOAIR743) \ No newline at end of file diff --git a/src/main/target/MICOAIR743/config.c b/src/main/target/MICOAIR743/config.c new file mode 100644 index 00000000000..6f5be655693 --- /dev/null +++ b/src/main/target/MICOAIR743/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" +#include "fc/config.h" +#include "sensors/gyro.h" + + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_MSP_OSD; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_ESCSERIAL; +} diff --git a/src/main/target/MICOAIR743/target.c b/src/main/target/MICOAIR743/target.c new file mode 100644 index 00000000000..139fad35772 --- /dev/null +++ b/src/main/target/MICOAIR743/target.c @@ -0,0 +1,42 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h new file mode 100644 index 00000000000..4ba27f3447e --- /dev/null +++ b/src/main/target/MICOAIR743/target.h @@ -0,0 +1,141 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "M743" + +#define USBD_PRODUCT_STRING "MICOAIR743" + +// *************** LED ********************** +#define LED0 PE5 +#define LED1 PE6 +#define LED2 PE4 + +// *************** SPI: Gyro & ACC & OSD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PD3 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_IMU_BMI088 +#define IMU_BMI088_ALIGN CW270_DEG +#define BMI088_SPI_BUS BUS_SPI2 +#define BMI088_GYRO_CS_PIN PD5 +#define BMI088_ACC_CS_PIN PD4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PD0 + +#define USE_UART7 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 //VCP, UART1, UART2, UART3, UART4, UART6, UART7, UART8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** I2C: BARO & MAG **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define USE_I2C_DEVICE_2 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C2 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +// *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 2121 +#define CURRENT_METER_SCALE 402 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/common.h b/src/main/target/common.h index 8bb735fc437..51f1fbf6ea6 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -195,6 +195,11 @@ #define USE_HEADTRACKER_SERIAL #define USE_HEADTRACKER_MSP +#if defined(STM32F7) || defined(STM32H7) +// needs bi-direction inverter, not available on F4 hardware. +#define USE_TELEMETRY_SBUS2 +#endif + //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) #define USE_VTX_FFPV diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c new file mode 100644 index 00000000000..dc5fb81d77d --- /dev/null +++ b/src/main/telemetry/sbus2.c @@ -0,0 +1,205 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/utils.h" +#include "common/time.h" +#include "common/axis.h" + +#include "telemetry/telemetry.h" +#include "telemetry/sbus2.h" +#include "telemetry/sbus2_sensors.h" + +#include "rx/sbus.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" +#include "sensors/temperature.h" +#include "sensors/diagnostics.h" + +#include "io/gps.h" + +#include "navigation/navigation.h" + +#ifdef USE_ESC_SENSOR +#include "sensors/esc_sensor.h" +#include "flight/mixer.h" +#endif + +#ifdef USE_TELEMETRY_SBUS2 + +const uint8_t sbus2SlotIds[SBUS2_SLOT_COUNT] = { + 0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, + 0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3, + 0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB, + 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB +}; + +sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT] = {}; +uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT] = {}; +static uint8_t currentSlot = 0; +static timeUs_t nextSlotTime = 0; + +void handleSbus2Telemetry(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + float voltage = getBatteryVoltage() * 0.01f; + float cellVoltage = getBatteryAverageCellVoltage() * 0.01f; + float current = getAmperage() * 0.01f; + float capacity = getMAhDrawn(); + float altitude = getEstimatedActualPosition(Z) * 0.01f; + float vario = getEstimatedActualVelocity(Z); + float temperature = 0; + uint32_t rpm = 0; + +#ifdef USE_ESC_SENSOR + escSensorData_t * escSensor = escSensorGetData(); + if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { + rpm = escSensor->rpm; + temperature = escSensor->temperature; + } else { + rpm = 0; + temperature = 0; + } +#endif + + //temperature = 42.16f; + + DEBUG_SET(DEBUG_SBUS2, 0, voltage); + DEBUG_SET(DEBUG_SBUS2, 1, cellVoltage); + DEBUG_SET(DEBUG_SBUS2, 2, current); + DEBUG_SET(DEBUG_SBUS2, 3, capacity); + DEBUG_SET(DEBUG_SBUS2, 4, altitude); + DEBUG_SET(DEBUG_SBUS2, 5, vario); + DEBUG_SET(DEBUG_SBUS2, 6, rpm); + DEBUG_SET(DEBUG_SBUS2, 7, temperature); + + // 2 slots + send_voltagef(1, voltage, cellVoltage); + // 3 slots + send_s1678_currentf(3, current, capacity, voltage); + // 1 slot + send_RPM(6, rpm); + // 1 slot - esc temp + static int change = 0; + change++; + int delta = change / 10; + delta = delta % 20; + send_SBS01T(7, temperature); + + // 8 slots, gps + uint16_t speed = 0; + float latitude = 0; + float longitude = 0; + +#ifdef USE_GPS + if (gpsSol.fixType >= GPS_FIX_2D) { + speed = (CMSEC_TO_KPH(gpsSol.groundSpeed) + 0.5f); + latitude = gpsSol.llh.lat * 1e-7; + longitude = gpsSol.llh.lon * 1e-7; + } +#endif + + send_F1675f(8, speed, altitude, vario, latitude, longitude); + // imu 1 slot + int16_t temp16; + bool valid = getIMUTemperature(&temp16); + send_SBS01T(16, valid ? temp16 / 10 : 0); + // baro + valid = 0; + valid = getBaroTemperature(&temp16); + send_SBS01T(17, valid ? temp16 / 10 : 0); + + // temp sensors 18-25 +#ifdef USE_TEMPERATURE_SENSOR + for(int i = 0; i < 8; i++) { + temp16 = 0; + valid = getSensorTemperature(0, &temp16); + send_SBS01T(18 + i, valid ? temp16 / 10 : 0); + } +#endif +} + +uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed) +{ + UNUSED(elapsed); + if (elapsed < SBUS2_DEADTIME) { + currentSlot = 0; + nextSlotTime = 0; + return 0xFF; // skip it + } + + if(currentSlot < SBUS2_TELEMETRY_SLOTS) { + return currentSlot; + } + + return 0xFF; +} + +void sbus2IncrementTelemetrySlot(timeUs_t currentTimeUs) +{ + nextSlotTime = currentTimeUs + (SBUS2_TRANSMIT_TIME + SBUS2_SLOT_DELAY); + currentSlot++; +} + +FAST_CODE void taskSendSbus2Telemetry(timeUs_t currentTimeUs) +{ + if (!telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL || + rxConfig()->serialrx_provider != SERIALRX_SBUS2) { + return; + } + + timeUs_t elapsedTime = currentTimeUs - sbusGetLastFrameTime(); + + if (elapsedTime > MS2US(8)) { + currentSlot = 0; + nextSlotTime = 0; + return; + } + + if (currentTimeUs < nextSlotTime) { + return; + } + + uint8_t telemetryPage = sbusGetCurrentTelemetryPage(); + + uint8_t slot = sbus2GetTelemetrySlot(elapsedTime); + + if(slot < SBUS2_TELEMETRY_SLOTS) { + int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + slot; + if (slotIndex < SBUS2_SLOT_COUNT) { + if (sbusTelemetryDataUsed[slotIndex] != 0) { + // send + serialWriteBuf(telemetrySharedPort, + (const uint8_t *)&sbusTelemetryData[slotIndex], + sizeof(sbus2_telemetry_frame_t)); + } + } + sbus2IncrementTelemetrySlot(currentTimeUs); + } +} + + + + +#endif diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h new file mode 100644 index 00000000000..0fc74f3298d --- /dev/null +++ b/src/main/telemetry/sbus2.h @@ -0,0 +1,70 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#include + +#include "platform.h" + +#include "common/time.h" +#include "common/utils.h" + +#define SBUS2_TELEMETRY_PAYLOAD_SIZE 3 + +#define SBUS2_TELEMETRY_ITEM_SIZE 3 +#define SBUS2_TELEMETRY_SLOTS 8 +#define SBUS2_TELEMETRY_PAGES 4 + +#define SBUS2_DEADTIME MS2US(2) +#define SBUS2_SLOT_TIME 650u + +#define SBUS2_TRANSMIT_TIME ((8 + 1 + 2 + 1 + 1) * 3 * 10) // 8e2, 100000 baud + star and stop bits +#define SBUS2_SLOT_DELAY 200 + +#define SBUS2_SLOT_COUNT (SBUS2_TELEMETRY_PAGES * SBUS2_TELEMETRY_SLOTS) + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) + +// Information on SBUS2 sensors from: https://github.com/BrushlessPower/SBUS2-Telemetry/tree/master +typedef struct sbus2_telemetry_frame_s { + uint8_t slotId; + union + { + uint8_t data[2]; + uint16_t u16; + } payload; +} __attribute__((packed)) sbus2_telemetry_frame_t; + + +STATIC_ASSERT(sizeof(sbus2_telemetry_frame_t) == 3, sbus2_telemetry_size); + +extern const uint8_t sbus2SlotIds[SBUS2_SLOT_COUNT]; +extern sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT]; +extern uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT]; + +// refresh telemetry buffers +void handleSbus2Telemetry(timeUs_t currentTimeUs); + +// time critical, send sbus2 data +void taskSendSbus2Telemetry(timeUs_t currentTimeUs); + +uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed); +void sbus2IncrementTelemetrySlot(timeUs_t now); + +#endif diff --git a/src/main/telemetry/sbus2_sensors.c b/src/main/telemetry/sbus2_sensors.c new file mode 100644 index 00000000000..f2e94f51ac1 --- /dev/null +++ b/src/main/telemetry/sbus2_sensors.c @@ -0,0 +1,732 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + +#include "telemetry/sbus2.h" +#include "telemetry/sbus2_sensors.h" + + +#ifdef USE_TELEMETRY_SBUS2 +void send_RPM(uint8_t port, uint32_t RPM) +{ + uint32_t value = 0; + uint8_t bytes[2] = { }; + + value = RPM / 6; + if(value > 0xffff){ + value = 0xffff; + } + bytes[1] = value >> 8; + bytes[0] = value; + SBUS2_transmit_telemetry_data( port , bytes); +} + + +void send_temp125(uint8_t port, int16_t temp) +{ + int16_t value= 0; + uint8_t bytes[2] = { }; + + value = temp | 0x4000; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); +} +void send_SBS01T(uint8_t port, int16_t temp){ + int16_t value= 0; + uint8_t bytes[2] = {}; + + value = temp | 0x8000; + value = value + 100; + bytes[0] = value;// >> 8; + bytes[1] = value >> 8; + SBUS2_transmit_telemetry_data(port , bytes); +} + +void send_voltage(uint8_t port,uint16_t voltage1, uint16_t voltage2) +{ + uint16_t value = 0; + uint8_t bytes[2] = { }; + + // VOLTAGE1 + value = voltage1 | 0x8000; + if ( value > 0x9FFF ){ + value = 0x9FFF; // max voltage is 819.1 + } + else if(value < 0x8000){ + value = 0x8000; + } + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); + + //VOLTAGE2 + value = voltage2; + if ( value > 0x1FFF ){ + value = 0x1FFF; // max voltage is 819.1 + } + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port+1 , bytes); +} + +void send_s1678_current(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage) +{ + uint16_t value = 0; + uint32_t local = 0; + uint8_t bytes[2] = { }; + + + // CURRENT + local = ((uint32_t)current) * 1 ; + value = (uint16_t)local; + if ( value > 0x3FFF ) + { + // max current is 163.83 + value = 0x3FFF; + } + bytes[0] = value >> 8; + bytes[0] = bytes[0] | 0x40; + bytes[0] = bytes[0] & 0x7F; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); + + //VOLTAGE + local = ((uint32_t)voltage) * 1; + value = (uint16_t)local; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port+1 , bytes); + + // CAPACITY + local = (uint32_t)capacity; + value = (uint16_t)local; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port+2 , bytes); +} + +void send_f1675_gps(uint8_t port, uint16_t speed, int16_t altitude, int16_t vario, int32_t latitude, int32_t longitude) +{ + uint16_t value1 = 0; + int16_t value2 = 0; + uint32_t value3 = 0; + bool latitude_pos = false; + bool longitude_pos = false; + uint8_t bytes[2] = {}; + + + // SPEED -> Bit 14(bytes[1] bit7) -> GPS Valid or not + value1 = speed | 0x4000; + if (value1 > 0x43E7 ){ + value1 = 0x43E7; // max Speed is 999 km/h + } + else if( value1 < 0x4000){ + value1 = 0x4000; + } + bytes[0] = value1 >> 8; + bytes[1] = value1; + SBUS2_transmit_telemetry_data( port , bytes); + + //HIGHT + value2 = altitude | 0x4000; + /*if(value2 > 0x7FFF ){ // max = +16383 + value2 = 0x7FFF; + } + else if( value2 < 0xC000){ // min = -16384 + value2 = 0xC000; + }*/ + bytes[0] = value2 >> 8; + bytes[1] = value2; + SBUS2_transmit_telemetry_data( port+1 , bytes); + + //TIME -> 12:34:56 Uhr = 12*60*60 + 34*60 + 56 = 45296 = 0xB0F0 + bytes[0] = 0xB0; + bytes[1] = 0xF0; + SBUS2_transmit_telemetry_data( port+2 , bytes); + + // VARIO + value2 = vario * 10; + bytes[0] = value2 >> 8; + bytes[1] = value2; + SBUS2_transmit_telemetry_data( port+3 , bytes); + + // LATITUDE + if(latitude >= 0){ + latitude_pos = true; + } + else{ + latitude_pos = false; + latitude = latitude * -1; + } + bytes[0] = (uint8_t)(latitude/1000000); + value3 = (uint32_t)(latitude%1000000); + if(latitude_pos){ + bytes[1] = ((value3 >> 16) & 0x0f); // North + } + else{ + bytes[1] = ((value3 >> 16) | 0x1f); // South + } + SBUS2_transmit_telemetry_data( port+4 , bytes); + + bytes[0] = ((value3 >> 8) & 0xff); + bytes[1] = value3 & 0xff; + SBUS2_transmit_telemetry_data( port+5 , bytes); + + // LONGITUDE + if(longitude >= 0){ + longitude_pos = true; + } + else{ + longitude_pos = false; + longitude = longitude * -1; + } + bytes[0] = (uint8_t)(longitude/1000000); + value3 = (uint32_t)(longitude%1000000); + if(longitude_pos){ + bytes[1] = ((value3 >> 16) & 0x0f); // Eath + } + else{ + bytes[1] = ((value3 >> 16) | 0x1f); // West + } + SBUS2_transmit_telemetry_data( port+6 , bytes); + + bytes[0] = ((value3 >> 8) & 0xff); + bytes[1] = value3 & 0xff; + SBUS2_transmit_telemetry_data( port+7 , bytes); +} + +void send_f1672_vario(uint8_t port, int16_t altitude, int16_t vario) +{ + int16_t value = 0; + uint8_t bytes[2] = { }; + + // VARIO + value = vario; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port, bytes); + + //HIGHT + value = altitude | 0x4000; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 1, bytes); +} + +void send_f1712_vario(uint8_t port, int16_t altitude, int16_t vario) +{ + int16_t value = 0; + uint8_t bytes[2] = { }; + + // VARIO + value = vario; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); + + //HIGHT + value = altitude | 0x4000; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 1 , bytes); + +} + + +void send_SBS01TE(uint8_t port, int16_t temp){ + send_temp125(port, temp); +} +void send_F1713(uint8_t port, int16_t temp){ + send_temp125(port, temp); +} + +void send_SBS01RB(uint8_t port, uint32_t RPM){ + send_RPM(port, RPM); +} +void send_SBS01RM(uint8_t port, uint32_t RPM){ + send_RPM(port, RPM); +} +void send_SBS01RO(uint8_t port, uint32_t RPM){ + send_RPM(port, RPM); +} +void send_SBS01R(uint8_t port, uint32_t RPM){ + send_RPM(port, RPM); +} + +void send_F1678(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){ + send_s1678_current(port, current, capacity, voltage); +} +void send_s1678_currentf(uint8_t port, float current, uint16_t capacity, float voltage){ + send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100)); +} +void send_F1678f(uint8_t port, float current, uint16_t capacity, float voltage){ + send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100)); +} +void send_SBS01V(uint8_t port,uint16_t voltage1, uint16_t voltage2){ + send_voltage(port, voltage1, voltage2); +} +void send_SBS01Vf(uint8_t port,float voltage1, float voltage2){ + send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10)); +} +void send_voltagef(uint8_t port,float voltage1, float voltage2){ + send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10)); +} +void send_SBS01C(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){ + send_s1678_current(port, current, capacity, voltage); +} +void send_SBS01Cf(uint8_t port, float current, uint16_t capacity, float voltage){ + send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100)); +} + +void send_f1712_variof(uint8_t port, int16_t altitude, float vario){ + send_f1712_vario(port, altitude, (int16_t)(vario * 10)); +} +void send_f1672_variof(uint8_t port, int16_t altitude, float vario){ + send_f1672_vario(port, altitude, (int16_t)(vario * 100)); +} +void send_F1712(uint8_t port, int16_t altitude, int16_t vario){ + send_f1712_vario(port, altitude, vario); +} +void send_F1712f(uint8_t port, int16_t altitude, float vario){ + send_f1712_vario(port, altitude, (int16_t)(vario * 10)); +} +void send_F1672(uint8_t port, int16_t altitude, int16_t vario){ + send_f1672_vario(port, altitude, vario); +} +void send_F1672f(uint8_t port, int16_t altitude, float vario){ + send_f1672_vario(port, altitude, (int16_t)(vario * 100)); +} + +void send_F1675minf(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, float lat_min, int8_t lon_deg, float lon_min){ + bool Lat_Negative = false; + bool Lon_Negative = false; + if(lat_deg < 0){ + Lat_Negative = true; + lat_deg = lat_deg * -1; + } + if(lon_deg < 0){ + Lon_Negative = true; + lon_deg = lon_deg * -1; + } + if(lat_min < 0){ + Lat_Negative = true; + lat_min = lat_min * -1; + } + if(lon_min < 0){ + Lon_Negative = true; + lon_min = lon_min * -1; + } + int32_t _latitude_deg = lat_deg; + int32_t _longitude_deg = lon_deg; + int32_t _latitude_min = lat_min * 10000; + int32_t _longitude_min = lon_min * 10000; + int32_t _latitude = _latitude_deg * 1000000; + int32_t _longitude = _longitude_deg * 1000000; + _latitude = _latitude + _latitude_min; + _longitude = _longitude + _longitude_min; + if(Lat_Negative){ + _latitude = _latitude * -1; + } + if(Lon_Negative){ + _longitude = _longitude * -1; + } + send_f1675_gps(port, speed, hight, vario, _latitude, _longitude); +} +void send_F1675min(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, int32_t lat_min, int8_t lon_deg, int32_t lon_min){ + bool Lat_Negative = false; + bool Lon_Negative = false; + if(lat_deg < 0){ + Lat_Negative = true; + lat_deg = lat_deg * -1; + } + if(lon_deg < 0){ + Lon_Negative = true; + lon_deg = lon_deg * -1; + } + if(lat_min < 0){ + Lat_Negative = true; + lat_min = lat_min * -1; + } + if(lon_min < 0){ + Lon_Negative = true; + lon_min = lon_min * -1; + } + int32_t _latitude_deg = lat_deg; + int32_t _longitude_deg = lon_deg; + int32_t _latitude = _latitude_deg * 1000000; + int32_t _longitude = _longitude_deg * 1000000; + _latitude = _latitude + lat_min; + _longitude = _longitude + lon_min; + if(Lat_Negative){ + _latitude = _latitude * -1; + } + if(Lon_Negative){ + _longitude = _longitude * -1; + } + send_f1675_gps(port, speed, hight, vario, _latitude, _longitude); +} +void send_F1675(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int32_t latitude, int32_t longitude){ + int32_t _latitude = latitude; + int32_t _longitude = longitude; + int32_t _latitude_deg = _latitude/1000000; + int32_t _longitude_deg = _longitude/1000000; + int32_t _latitude_min = _latitude%1000000; + int32_t _longitude_min = _longitude%1000000; + _latitude = _latitude_deg * 1000000; + _longitude = _longitude_deg * 1000000; + _latitude = _latitude + ((_latitude_min * 60)/100); + _longitude = _longitude + ((_longitude_min * 60)/100); + send_f1675_gps(port, speed, hight, vario, _latitude, _longitude); +} +void send_F1675f(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, float latitude, float longitude){ + int32_t _latitude = latitude * 1000000; + int32_t _longitude = longitude * 1000000; + int32_t _latitude_deg = _latitude/1000000; + int32_t _longitude_deg = _longitude/1000000; + int32_t _latitude_min = _latitude%1000000; + int32_t _longitude_min = _longitude%1000000; + _latitude = _latitude_deg * 1000000; + _longitude = _longitude_deg * 1000000; + _latitude = _latitude + ((_latitude_min * 60)/100); + _longitude = _longitude + ((_longitude_min * 60)/100); + send_f1675_gps(port, speed, hight, vario, _latitude, _longitude); +} +void send_SBS10G( + uint8_t port, + uint16_t hours, // 0 to 24 + uint16_t minutes, // 0 to 60 + uint16_t seconds, // 0 to 60 + float latitude, // decimal degrees (i.e. 52.520833; negative value for southern hemisphere) + float longitude, // decimal degrees (i.e. 13.409430; negative value for western hemisphere) + float altitudeMeters, // meters (valid range: -1050 to 4600) + uint16_t speed, // km/h (valid range 0 to 511) + float gpsVario) // m/s (valid range: -150 to 260) +{ + uint32_t utc = (hours*3600) + (minutes*60) + seconds; + uint32_t lat, lon; + // scale latitude/longitude (add 0.5 for correct rounding) + if (latitude > 0) { + lat = (600000.0*latitude) + 0.5; + } + else { + lat = (-600000.0*latitude) + 0.5; + // toggle south bit + lat |= 0x4000000; + } + if (longitude > 0) { + lon = (600000.0*longitude) + 0.5; + } + else { + lon = (-600000.0*longitude) + 0.5; + // toggle west bit + lon |= 0x8000000; + } + // convert altitude (add 0.5 for correct rounding) + uint16_t alt = (altitudeMeters>=-820 && altitudeMeters<=4830) ?(1.25*(altitudeMeters+820)) + 0.5 : 0; + // error check speed + if (speed < 512) { + // set speed enable bit + speed |= 0x200; + } + else { + speed = 0; + } + // initialize buffer + uint8_t bytes[2] = { }; + // slot 0 (utc) + bytes[0] = (utc&0x00ff); + bytes[1] = (utc&0xff00)>>8; + SBUS2_transmit_telemetry_data(port , bytes); + // slot 1 (latitude & utc) + bytes[0] = ((lat&0x007f)<<1) | ((utc&0x10000)>>16); + bytes[1] = (lat&0x7f80)>>7; + SBUS2_transmit_telemetry_data(port+1 , bytes); + // slot 2 (latitude & longitude) + bytes[0] = (lat&0x07f8000)>>15; + bytes[1] = ((lat&0x7800000)>>23) | (lon&0x0f)<<4; + SBUS2_transmit_telemetry_data(port+2 , bytes); + // slot 3 (longitude) + bytes[0] = (lon&0x00ff0)>>4; + bytes[1] = (lon&0xff000)>>12; + SBUS2_transmit_telemetry_data(port+3 , bytes); + // slot 4 (longitude & speed) + bytes[0] = ((lon&0xff00000)>>20); + bytes[1] = (speed&0xff); + SBUS2_transmit_telemetry_data(port+4 , bytes); + // slot 5 (pressure & speed) + bytes[0] = ((speed&0x300)>>8); + bytes[1] = 0x00; + SBUS2_transmit_telemetry_data(port+5 , bytes); + // slot 6 (altitude & pressure) + bytes[0] = ((alt&0x003)<<6); + bytes[1] = (alt&0x3fc)>>2; + SBUS2_transmit_telemetry_data(port+6 , bytes); + // slot (7 (vario & altitude) + uint16_t vario; + // error check vario + if (gpsVario >= -150 && gpsVario <= 260) { + // scale vario (add 0.5 for correct rounding) + vario = (10.0*(gpsVario + 150)) + 0.5; + // set vario enable + vario |= 0x1000; + } + else { + vario = 0; + } + bytes[0] = ((vario&0x001f)<<3) | ((alt&0x1c00)>>10); + bytes[1] = (vario&0x1fe0)>>5; + SBUS2_transmit_telemetry_data(port+7 , bytes); +} +void send_scorpion_kontronik( + uint8_t port, + uint16_t voltage, + uint16_t capacity, + uint32_t rpm, + uint16_t current, + uint16_t temp, + uint16_t becTemp, + uint16_t becCurrent, + uint16_t pwm) +{ + uint32_t value = 0; + uint8_t bytes[2] = { }; + + // voltage 41.1 = 4110 + value = voltage | 0x8000; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); + + // 1330 mah => 1.33 Ah + value = capacity; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 1 , bytes); + + // 2250 rpm => 2250 + value = rpm / 6; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 2 , bytes); + + // 13310 => 133.1 A + value = current; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 3 , bytes); + + // 41 => 41 Celsius + value = temp; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 4 , bytes); + + // 21 => Bec Celsius + value = becTemp; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 5 , bytes); + + // 650 => 6,5 Bec Current + value = becCurrent; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 6 , bytes); + + // PWM output + value = pwm; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 7 , bytes); +} + +void send_scorpion( + uint8_t port, + uint16_t voltage, + uint16_t capacity, + uint32_t rpm, + uint16_t current, + uint16_t temp, + uint16_t becTemp, + uint16_t becCurrent, + uint16_t pwm) +{ + send_scorpion_kontronik( + port, + voltage, + capacity, + rpm, + current, + temp, + becTemp, + becCurrent, + pwm); +} + +void send_kontronik( + uint8_t port, + uint16_t voltage, + uint16_t capacity, + uint32_t rpm, + uint16_t current, + uint16_t temp, + uint16_t becTemp, + uint16_t becCurrent, + uint16_t pwm) +{ + send_scorpion_kontronik( + port, + voltage, + capacity, + rpm, + current, + temp, + becTemp, + becCurrent, + pwm); +} + +void send_jetcat( + uint8_t port, + uint32_t rpm, + uint16_t egt, + uint16_t pump_volt, + uint32_t setrpm, + uint16_t thrust, + uint16_t fuel, + uint16_t fuelflow, + uint16_t altitude, + uint16_t quality, + uint16_t volt, + uint16_t current, + uint16_t speed, + uint16_t status, + uint32_t secondrpm) +{ + uint32_t value = 0; + uint8_t bytes[2] = {}; + + // Actual RPM with 0x4000 Offset -> why? + value = rpm / 100; + value = value | 0x4000; + if(value > 0xffff){ + value = 0xffff; + } + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port , bytes); + + // EGT Abgastemperatur in °C + value = egt; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 1 , bytes); + + // Pump Voltage 12.34V = 1234 + value = pump_volt; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 2 , bytes); + + // Setpoint RPM without Offset + value = setrpm / 100; + if(value > 0xffff){ + value = 0xffff; + } + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 3 , bytes); + + // Thrust 123.4N = 1234 + value = thrust; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 4 , bytes); + + // Fuel (remain) in ml + value = fuel; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 5 , bytes); + + // Fuel Flow in ml/min + value = fuelflow; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 6 , bytes); + + // Altitude -> without offset? + value = altitude; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 7 , bytes); + + // Fuel Quality in % + value = quality; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 8 , bytes); + + // Voltage 12.34V = 1234 + value = volt; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 9 , bytes); + + // Current 123.4A = 1234 + value = current; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 10 , bytes); + + // Speed in km/h + value = speed; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 11 , bytes); + + // Status and Error Code + value = status; + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 12 , bytes); + + // Second RPM without Offset + value = secondrpm / 100; + if(value > 0xffff){ + value = 0xffff; + } + bytes[0] = value >> 8; + bytes[1] = value; + SBUS2_transmit_telemetry_data( port + 13 , bytes); + +} + + +void SBUS2_transmit_telemetry_data(uint8_t slotId , const uint8_t *bytes) +{ + if(slotId > 0 && slotId < SBUS2_SLOT_COUNT) { + sbusTelemetryData[slotId].payload.data[0] = bytes[0]; + sbusTelemetryData[slotId].payload.data[1] = bytes[1]; + sbusTelemetryData[slotId].slotId = sbus2SlotIds[slotId]; + //sbusTelemetryData[i].payload.data[0] = 0x81; + //sbusTelemetryData[i].payload.data[1] = 0x80; + sbusTelemetryDataUsed[slotId] = 1; + } + +} +#endif \ No newline at end of file diff --git a/src/main/telemetry/sbus2_sensors.h b/src/main/telemetry/sbus2_sensors.h new file mode 100644 index 00000000000..53ffb07aded --- /dev/null +++ b/src/main/telemetry/sbus2_sensors.h @@ -0,0 +1,121 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#include + +#include "platform.h" + +#ifdef USE_TELEMETRY_SBUS2 + +// Sensor code from https://github.com/BrushlessPower/SBUS2-Telemetry +// SBUS2 telemetry: 2ms deadtime after rc package +// One new slot every 700us + +/* + * ++++++++++++++++++++++++++++++++ + * Temperature Sensors + * ++++++++++++++++++++++++++++++++ + */ +void send_temp125(uint8_t port, int16_t temp); +void send_alarm_as_temp125(uint8_t port, int16_t alarm); +void send_SBS01TE(uint8_t port, int16_t temp); +void send_SBS01T(uint8_t port, int16_t temp); +void send_F1713(uint8_t port, int16_t temp); + +/* + * ++++++++++++++++++++++++++++++++ + * RPM Sensors + * ++++++++++++++++++++++++++++++++ + */ +void send_RPM(uint8_t port, uint32_t RPM); +void send_SBS01RB(uint8_t port, uint32_t RPM); +void send_SBS01RM(uint8_t port, uint32_t RPM); +void send_SBS01RO(uint8_t port, uint32_t RPM); +void send_SBS01R(uint8_t port, uint32_t RPM); + +/* + * ++++++++++++++++++++++++++++++++ + * Voltage/Current Sensors + * ++++++++++++++++++++++++++++++++ + */ +void send_voltage(uint8_t port,uint16_t voltage1, uint16_t voltage2); +void send_voltagef(uint8_t port,float voltage1, float voltage2); +void send_s1678_current(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage); +void send_s1678_currentf(uint8_t port, float current, uint16_t capacity, float voltage); +void send_SBS01C(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage); +void send_SBS01Cf(uint8_t port, float current, uint16_t capacity, float voltage); +void send_F1678(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage); +void send_F1678f(uint8_t port, float current, uint16_t capacity, float voltage); +void send_SBS01V(uint8_t port,uint16_t voltage1, uint16_t voltage2); +void send_SBS01Vf(uint8_t port,float voltage1, float voltage2); + + +/* + * ++++++++++++++++++++++++++++++++ + * Vario Sensors + * ++++++++++++++++++++++++++++++++ + */ +void send_f1712_vario(uint8_t port, int16_t altitude, int16_t vario); +void send_f1712_variof(uint8_t port, int16_t altitude, float vario); +void send_f1672_vario(uint8_t port, int16_t altitude, int16_t vario); +void send_f1672_variof(uint8_t port, int16_t altitude, float vario); +void send_F1712(uint8_t port, int16_t altitude, int16_t vario); +void send_F1712f(uint8_t port, int16_t altitude, float vario); +void send_F1672(uint8_t port, int16_t altitude, int16_t vario); +void send_F1672f(uint8_t port, int16_t altitude, float vario); + +/* + * ++++++++++++++++++++++++++++++++ + * GPS Sensors + * Note the different Input Types! + * Example: + * Position Berlin Fernsehturm + * https://www.koordinaten-umrechner.de/decimal/52.520832,13.409430?karte=OpenStreetMap&zoom=19 + * Degree Minutes 52° 31.2499 and 13° 24.5658 + * Decimal Degree 52.520832 and 13.409430 + * ++++++++++++++++++++++++++++++++ + */ +// Degree Minutes as Integer -> 52312499 +void send_f1675_gps(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int32_t latitude, int32_t longitude); +// Degree Minutes as Integer -> 52 and 312499 +void send_F1675min(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, int32_t lat_min, int8_t lon_deg, int32_t lon_min); +// Degree Minutes as Float -> 52 and 31.2499 +void send_F1675minf(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, float lat_min, int8_t lon_deg, float lon_min); +// Decimal Degrees as Float -> 52.520832 +void send_F1675f(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, float latitude, float longitude); +// Decimal Degrees as Integer -> 52520832 +void send_F1675(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int32_t latitude, int32_t longitude); +void send_SBS10G(uint8_t port, uint16_t hours, uint16_t minutes, uint16_t seconds, float latitude, float longitude, float altitudeMeters, uint16_t speed, float gpsVario); + +/* + * ++++++++++++++++++++++++++++++++ + * ESC Sensors + * Note These sensors only exists on the newer Futaba Radios 18SZ, 16IZ, etc + * ++++++++++++++++++++++++++++++++ + */ + +void send_kontronik(uint8_t port, uint16_t voltage, uint16_t capacity, uint32_t rpm, uint16_t current, uint16_t temp, uint16_t becTemp, uint16_t becCurrent, uint16_t pwm); +void send_scorpion(uint8_t port, uint16_t voltage, uint16_t capacity, uint32_t rpm, uint16_t current, uint16_t temp, uint16_t becTemp, uint16_t becCurrent, uint16_t pwm); + +void send_jetcat(uint8_t port, uint32_t rpm, uint16_t egt, uint16_t pump_volt, uint32_t setrpm, uint16_t thrust, uint16_t fuel, uint16_t fuelflow, uint16_t altitude, uint16_t quality, uint16_t volt, uint16_t current, uint16_t speed, uint16_t status, uint32_t secondrpm); + +void SBUS2_transmit_telemetry_data(uint8_t slotId , const uint8_t *bytes); + +#endif \ No newline at end of file diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d6c8e355d86..56c18ca159b 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -23,6 +23,8 @@ #ifdef USE_TELEMETRY +#include "build/debug.h" + #include "common/utils.h" #include "config/parameter_group.h" @@ -49,6 +51,7 @@ #include "telemetry/ibus.h" #include "telemetry/crsf.h" #include "telemetry/srxl.h" +#include "telemetry/sbus2.h" #include "telemetry/sim.h" #include "telemetry/ghst.h" @@ -245,6 +248,10 @@ void telemetryProcess(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY_GHST handleGhstTelemetry(currentTimeUs); #endif + +#ifdef USE_TELEMETRY_SBUS2 + handleSbus2Telemetry(currentTimeUs); +#endif } #endif