update jeti telemetry (#5030)

* update jeti telemetry

* add new values: Power, Roll angle, Pitch angle, Heading
* optimized reading of the values

* Indentation corrected

* correction and formating updated

* code formating, renaming, ...

* remove extra whitespace
* corrected comment formating
* adapt code for betaflight coding standards
* rename function for consistency

* code formating...

* Changes from Ledvinap

* Revert "Changes from Ledvinap"

This reverts commit 3845ed98abddfcedcc11a1cdedc68a4993afd84a.

* applied changes from ledvinap

* rebase

* Revert "rebase"

This reverts commit be89a6428aa0034dc8ef1f701ab7d2d4f3e2f8ae.

* Revert "Revert "rebase""

This reverts commit b16c9e9adf7a9fa321716929bd9f20b60f1f41d2.

src/main/telemetry/jetiexbus.c

@@ -27,17 +27,19 @@
 
 #include "build/build_config.h"
 #include "build/debug.h"
+#include "fc/runtime_config.h"
 
 #include "common/utils.h"
+#include "common/bitarray.h"
 
 #include "drivers/serial.h"
 #include "drivers/serial_uart.h"
 #include "drivers/time.h"
 
 #include "flight/altitude.h"
+#include "flight/imu.h"
 
 #include "io/serial.h"
-
 #include "rx/rx.h"
 #include "rx/jetiexbus.h"
 
@@ -47,19 +49,12 @@
 #include "telemetry/jetiexbus.h"
 #include "telemetry/telemetry.h"
 
-
-#define EXBUS_HEADER_LEN                6
-#define EXBUS_CRC_LEN                   2
-#define EXBUS_OVERHEAD                  (EXBUS_HEADER_LEN + EXBUS_CRC_LEN)
-#define EXBUS_MAX_CHANNEL_FRAME_SIZE    (EXBUS_HEADER_LEN + JETIEXBUS_CHANNEL_COUNT*2 + EXBUS_CRC_LEN)
-#define EXBUS_MAX_REQUEST_FRAME_SIZE    9
-
 #define EXTEL_DATA_MSG      (0x40)
 #define EXTEL_UNMASK_TYPE   (0x3F)
 #define EXTEL_SYNC_LEN      1
 #define EXTEL_CRC_LEN       1
 #define EXTEL_HEADER_LEN    6
-#define EXTEL_MAX_LEN       29
+#define EXTEL_MAX_LEN       28
 #define EXTEL_OVERHEAD      (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN)
 #define EXTEL_MAX_PAYLOAD   (EXTEL_MAX_LEN - EXTEL_OVERHEAD)
 #define EXBUS_MAX_REQUEST_BUFFER_SIZE   (EXBUS_OVERHEAD + EXTEL_MAX_LEN)
@@ -85,15 +80,16 @@ enum {
 };
 
 enum exDataType_e {
-    EX_TYPE_6b   = 0, // int6_t  Data type 6b (-31 ¸31)
+    EX_TYPE_6b   = 0,                // int6_t  Data type 6b (-31 ¸31)
-    EX_TYPE_14b  = 1, // int14_t Data type 14b (-8191 ¸8191)
+    EX_TYPE_14b  = 1,                // int14_t Data type 14b (-8191 ¸8191)
-    EX_TYPE_22b  = 4, // int22_t Data type 22b (-2097151 ¸2097151)
+    EX_TYPE_22b  = 4,                // int22_t Data type 22b (-2097151 ¸2097151)
-    EX_TYPE_DT   = 5, // int22_t Special data type – time and date
+    EX_TYPE_DT   = 5,                // int22_t Special data type – time and date
-    EX_TYPE_30b  = 8, // int30_t Data type 30b (-536870911 ¸536870911)
+    EX_TYPE_30b  = 8,                // int30_t Data type 30b (-536870911 ¸536870911)
-    EX_TYPE_GPS  = 9  // int30_t Special data type – GPS coordinates:  lo/hi minute - lo/hi degree.
+    EX_TYPE_GPS  = 9,                // int30_t Special data type – GPS coordinates:  lo/hi minute - lo/hi degree.
+    EX_TYPE_DES  = 255               // only for devicedescription
 };
 
-const uint8_t exDataTypeLen[]={
+const uint8_t exDataTypeLen[] = {
     [EX_TYPE_6b]  = 1,
     [EX_TYPE_14b] = 2,
     [EX_TYPE_22b] = 3,
@@ -102,10 +98,9 @@ const uint8_t exDataTypeLen[]={
     [EX_TYPE_GPS] = 4
 };
 
-typedef struct exBusSensor_s{
+typedef struct exBusSensor_s {
     const char *label;
     const char *unit;
-    int32_t value;
     const uint8_t exDataType;
     const uint8_t decimals;
 } exBusSensor_t;
@@ -113,15 +108,18 @@ typedef struct exBusSensor_s{
 #define DECIMAL_MASK(decimals) (decimals << 5)
 
 // list of telemetry messages
-// after every 15 sensors a new header has to be inserted (e.g. "CF-Dev 1.12 S2")
+// after every 15 sensors a new header has to be inserted (e.g. "BF D2")
-exBusSensor_t jetiExSensors[] = {
+const exBusSensor_t jetiExSensors[] = {
-    { "CF-Dev 1.12 S1", "",     0,      0,             0 },                     // device descripton
+    {"BF D1",       "",      EX_TYPE_DES,   0              },     // device descripton
-    { "Voltage",        "V",    0,      EX_TYPE_14b,   DECIMAL_MASK(1) },
+    {"Voltage",     "V",     EX_TYPE_14b,   DECIMAL_MASK(1)},
-    { "Current",        "A",    0,      EX_TYPE_14b,   DECIMAL_MASK(2) },
+    {"Current",     "A",     EX_TYPE_14b,   DECIMAL_MASK(2)},
-    { "Altitude",       "m",    0,      EX_TYPE_14b,   DECIMAL_MASK(1) },
+    {"Altitude",    "m",     EX_TYPE_14b,   DECIMAL_MASK(2)},
-    { "Capacity",       "mAh",  0,      EX_TYPE_22b,   DECIMAL_MASK(0) },
+    {"Capacity",    "mAh",   EX_TYPE_22b,   DECIMAL_MASK(0)},
-    { "frames lost",    " ",    0,      EX_TYPE_22b,   DECIMAL_MASK(0) },       // for debug only
+    {"Power",       "W",     EX_TYPE_22b,   DECIMAL_MASK(1)},
-    { "time Diff",      "us",   0,      EX_TYPE_14b,   DECIMAL_MASK(0) }        // for debug only
+    {"Roll angle",  "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
+    {"Pitch angle", "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
+    {"Heading",     "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
+    {"Vario",       "m/s",   EX_TYPE_22b,   DECIMAL_MASK(2)}
 };
 
 // after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description
@@ -130,33 +128,39 @@ enum exSensors_e {
     EX_CURRENT,
     EX_ALTITUDE,
     EX_CAPACITY,
-    EX_FRAMES_LOST,                                                             // for debug only
+    EX_POWER,
-    EX_TIME_DIFF                                                                // for debug only
+    EX_ROLL_ANGLE,
+    EX_PITCH_ANGLE,
+    EX_HEADING,
+    EX_VARIO
 };
 
 #define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors))
 
 static uint8_t jetiExBusTelemetryFrame[40];
 static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX;
-static void sendJetiExBusTelemetry(uint8_t packetID);
+static uint8_t firstActiveSensor = 0;
+static uint32_t exSensorEnabled = 0;
+
+static uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item);
+static uint8_t getNextActiveSensor(uint8_t currentSensor);
 
 // Jeti Ex Telemetry CRC calculations for a frame
 uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen)
 {
     uint8_t crc=0;
-
     for (uint8_t mlen = 0; mlen < msgLen; mlen++) {
         crc  ^= pt[mlen];
         crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18));
     }
     return(crc);
 }
-/*
-  -----------------------------------------------
-   Jeti Ex Bus Telemetry
-  -----------------------------------------------
-*/
 
+/*
+ * -----------------------------------------------
+ *  Jeti Ex Bus Telemetry
+ * -----------------------------------------------
+ */
 void initJetiExBusTelemetry(void)
 {
     // Init Ex Bus Frame header
@@ -164,18 +168,43 @@ void initJetiExBusTelemetry(void)
     jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01;
     jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A;    // Ex Telemetry
 
-
     // Init Ex Telemetry header
     uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
+
     jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F;              // Startbyte
     jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E;            // Serial Number 4 Byte
     jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4;
     jetiExTelemetryFrame[EXTEL_HEADER_LSN_LB] = 0x00;            // increment by telemetry count (%16) > only 15 values per device possible
     jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00;
     jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00;               // reserved, by default 0x00
+
+    //exSensorEnabled = 0x3fe;
+    // Check which sensors are available
+    if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
+        bitArraySet(&exSensorEnabled, EX_VOLTAGE);
+    }
+    if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
+        bitArraySet(&exSensorEnabled, EX_CURRENT);
+    }
+    if ((batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_NONE)) {
+        bitArraySet(&exSensorEnabled, EX_POWER);
+        bitArraySet(&exSensorEnabled, EX_CAPACITY);
+    }
+    if (sensors(SENSOR_BARO)) {
+        bitArraySet(&exSensorEnabled, EX_ALTITUDE);
+        bitArraySet(&exSensorEnabled, EX_VARIO);
+    }
+    if (sensors(SENSOR_ACC)) {
+        bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
+        bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE);
+    }
+    if (sensors(SENSOR_MAG)) {
+        bitArraySet(&exSensorEnabled, EX_HEADING);
+    }
+    firstActiveSensor = getNextActiveSensor(0);     // find the first active sensor
 }
 
-void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
+void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
 {
     uint8_t labelLength = strlen(sensor->label);
     uint8_t unitLength = strlen(sensor->unit);
@@ -191,27 +220,80 @@ void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const
     exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]);
 }
 
-uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart)
+int32_t getSensorValue(uint8_t sensor)
 {
-    uint8_t item = itemStart;
+    switch(sensor) {
+    case EX_VOLTAGE:
+        return getBatteryVoltageLatest();
+        break;
+
+    case EX_CURRENT:
+        return getAmperageLatest();
+        break;
+
+    case EX_ALTITUDE:
+        return getEstimatedAltitude();
+        break;
+
+    case EX_CAPACITY:
+        return getMAhDrawn();
+        break;
+
+    case EX_POWER:
+        return (getBatteryVoltageLatest() * getAmperageLatest() / 100);
+        break;
+
+    case EX_ROLL_ANGLE:
+        return attitude.values.roll;
+        break;
+
+    case EX_PITCH_ANGLE:
+        return attitude.values.pitch;
+        break;
+
+    case EX_HEADING:
+        return attitude.values.yaw;
+        break;
+
+    case EX_VARIO:
+        return getEstimatedVario();
+        break;
+
+    default:
+        return -1;
+    }
+}
+
+uint8_t getNextActiveSensor(uint8_t currentSensor)
+{
+    while( ++currentSensor < JETI_EX_SENSOR_COUNT) {
+        if (bitArrayGet(&exSensorEnabled, currentSensor)) {
+            break;
+        }
+    }
+    if (currentSensor == JETI_EX_SENSOR_COUNT ) {
+        currentSensor = firstActiveSensor;
+    }
+    return currentSensor;
+}
+
+uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item)
+{
+    uint8_t startItem = item;
+    uint8_t sensorItemMaxGroup = (item & 0xF0) + 0x10;
     uint8_t iCount;
     uint8_t messageSize;
     uint32_t sensorValue;
 
-    if ((item & 0x0F) == 0)
-        item++;
-
-    if (item >= JETI_EX_SENSOR_COUNT)
-        item = 1;
-
     exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0;                                   // Device ID
     uint8_t *p = &exMessage[EXTEL_HEADER_ID];
 
-    while (item <= (itemStart | 0x0F)) {
+    while (item < sensorItemMaxGroup) {
-        *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType;               // Sensor ID (%16) | EX Data Type
+        *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType;   // Sensor ID (%16) | EX Data Type
 
-        sensorValue = jetiExSensors[item].value;
+        sensorValue = getSensorValue(item);
         iCount = exDataTypeLen[jetiExSensors[item].exDataType];
+
         while (iCount > 1) {
             *p++ = sensorValue;
             sensorValue = sensorValue >> 8;
@@ -219,13 +301,16 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t itemStart)
         }
         *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals;
 
-        item++;
+        item = getNextActiveSensor(item);
-        if (item > JETI_EX_SENSOR_COUNT)
-            break;
-        if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1)
-            break;
-    }
 
+        if (startItem >= item) {
+            break;
+        }
+
+        if ((p - &exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType] + 1 >= EXTEL_MAX_PAYLOAD) {
+            break;
+        }
+    }
     messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID]));
     exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize;
     exMessage[messageSize + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], messageSize);
@@ -254,9 +339,10 @@ void checkJetiExBusTelemetryState(void)
 void handleJetiExBusTelemetry(void)
 {
     static uint16_t framesLost = 0; // only for debug
+    static uint8_t item = 0;
     uint32_t timeDiff;
-    // Check if we shall reset frame position due to time
 
+    // Check if we shall reset frame position due to time
     if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) {
 
         // to prevent timing issues from request to answer - max. 4ms
@@ -269,19 +355,13 @@ void handleJetiExBusTelemetry(void)
         }
 
         if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
-            jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
-            jetiExSensors[EX_CURRENT].value = getAmperage();
-            jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude();
-            jetiExSensors[EX_CAPACITY].value = getMAhDrawn();
-            jetiExSensors[EX_FRAMES_LOST].value = framesLost;
-            jetiExSensors[EX_TIME_DIFF].value = timeDiff;
-
             // switch to TX mode
             if (serialRxBytesWaiting(jetiExBusPort) == 0) {
                 serialSetMode(jetiExBusPort, MODE_TX);
                 jetiExBusTransceiveState = EXBUS_TRANS_TX;
-                sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID]);
+                item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item);
                 jetiExBusRequestState = EXBUS_STATE_PROCESSED;
+                return;
             }
         } else {
             jetiExBusRequestState = EXBUS_STATE_ZERO;
@@ -299,32 +379,39 @@ void handleJetiExBusTelemetry(void)
     }
 }
 
-void sendJetiExBusTelemetry(uint8_t packetID)
+uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
 {
-    static uint8_t sensorDescriptionCounter = 0;
+    static uint8_t sensorDescriptionCounter = 0xFF;
-    static uint8_t sensorValueCounter = 1;
+    static uint8_t requestLoop = 0xFF;
-    static uint8_t requestLoop = 0;
     uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
 
-    if (requestLoop == 100) {              //every nth request send the name of a value
+    if (requestLoop) {
-        if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT )
+        while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) {
+            if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter) || (jetiExSensors[sensorDescriptionCounter].exDataType == EX_TYPE_DES)) {
+                break;
+            }
+        }
+        if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) {
             sensorDescriptionCounter = 0;
+        }
 
-        createExTelemetrieTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]);
+        createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]);
         createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
-
+        requestLoop--;
-        requestLoop = 0;
+        if (requestLoop == 0){
-        sensorDescriptionCounter++;
+            item = firstActiveSensor;
+        }
     } else {
-        sensorValueCounter = createExTelemetrieValueMessage(jetiExTelemetryFrame, sensorValueCounter);
+        item = createExTelemetryValueMessage(jetiExTelemetryFrame, item);
         createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
     }
 
-    for (uint8_t iCount = 0; iCount < jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]; iCount++) {
+    serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]);
-        serialWrite(jetiExBusPort, jetiExBusTelemetryFrame[iCount]);
-    }
     jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED;
-    requestLoop++;
+
+    return item;
 }
 #endif // TELEMETRY
 #endif // SERIAL_RX
+
+