Merge remote-tracking branch 'upstream/master' into abo_nav_vel_z_imp…

…rovements

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 |

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

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)

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

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;
 

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

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

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();

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

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

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 {

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

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;

src/main/rx/rx.h

@@ -81,6 +81,7 @@ typedef enum {
     SERIALRX_GHST,
     SERIALRX_MAVLINK,
     SERIALRX_FBUS,
+    SERIALRX_SBUS2,
 } rxSerialReceiverType_e;
 
 #ifdef USE_24CHANNELS

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

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