From b16c9e9adf7a9fa321716929bd9f20b60f1f41d2 Mon Sep 17 00:00:00 2001 From: Thomas Miric Date: Wed, 31 Jan 2018 23:54:43 +0100 Subject: [PATCH] Revert "rebase" This reverts commit be89a6428aa0034dc8ef1f701ab7d2d4f3e2f8ae. --- src/main/telemetry/jetiexbus.c | 91 ++++++++++++++++------------------ 1 file changed, 42 insertions(+), 49 deletions(-) diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 6357b36100..172ce91789 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -40,6 +40,7 @@ #include "flight/imu.h" #include "io/serial.h" + #include "rx/rx.h" #include "rx/jetiexbus.h" @@ -54,11 +55,12 @@ #define EXTEL_SYNC_LEN 1 #define EXTEL_CRC_LEN 1 #define EXTEL_HEADER_LEN 6 -#define EXTEL_MAX_LEN 28 +#define EXTEL_MAX_LEN 29 #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) + enum exTelHeader_e { EXTEL_HEADER_SYNC = 0, EXTEL_HEADER_TYPE_LEN, @@ -80,6 +82,7 @@ enum { }; enum exDataType_e { + EX_TYPE_NONE = 0, EX_TYPE_6b = 0, // int6_t Data type 6b (-31 ¸31) EX_TYPE_14b = 1, // int14_t Data type 14b (-8191 ¸8191) EX_TYPE_22b = 4, // int22_t Data type 22b (-2097151 ¸2097151) @@ -118,8 +121,7 @@ const exBusSensor_t jetiExSensors[] = { {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, {"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)} + {"Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)} }; // after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description @@ -131,24 +133,24 @@ enum exSensors_e { EX_POWER, EX_ROLL_ANGLE, EX_PITCH_ANGLE, - EX_HEADING, - EX_VARIO + EX_HEADING }; #define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors)) static uint8_t jetiExBusTelemetryFrame[40]; static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX; -static uint8_t firstActiveSensor = 0; -static uint32_t exSensorEnabled = 0; +static uint8_t activeSensorList[JETI_EX_SENSOR_COUNT]; +static uint8_t activeSensors; +static uint32_t exSensorEnabled; 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)); @@ -170,7 +172,6 @@ void initJetiExBusTelemetry(void) // 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; @@ -178,7 +179,8 @@ void initJetiExBusTelemetry(void) jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 - //exSensorEnabled = 0x3fe; + + bitArraySet(&exSensorEnabled, 0); // Check which sensors are available if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { bitArraySet(&exSensorEnabled, EX_VOLTAGE); @@ -192,7 +194,6 @@ void initJetiExBusTelemetry(void) } if (sensors(SENSOR_BARO)) { bitArraySet(&exSensorEnabled, EX_ALTITUDE); - bitArraySet(&exSensorEnabled, EX_VARIO); } if (sensors(SENSOR_ACC)) { bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE); @@ -201,7 +202,13 @@ void initJetiExBusTelemetry(void) if (sensors(SENSOR_MAG)) { bitArraySet(&exSensorEnabled, EX_HEADING); } - firstActiveSensor = getNextActiveSensor(0); // find the first active sensor + + for (uint8_t item = 0; item < JETI_EX_SENSOR_COUNT; item++ ) { + if (bitArrayGet(&exSensorEnabled, item)) { + activeSensorList[activeSensors] = item; + activeSensors++; + } + } } void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) @@ -255,62 +262,48 @@ int32_t getSensorValue(uint8_t sensor) 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 sensorItem = activeSensorList[item]; + uint8_t sensorItemMaxGroup = sensorItem | 0xF0; uint8_t iCount; uint8_t messageSize; uint32_t sensorValue; + exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID uint8_t *p = &exMessage[EXTEL_HEADER_ID]; - while (item < sensorItemMaxGroup) { - *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type - - sensorValue = getSensorValue(item); - iCount = exDataTypeLen[jetiExSensors[item].exDataType]; + while (sensorItem <= sensorItemMaxGroup) { + *p++ = ((sensorItem & 0x0F) << 4) | jetiExSensors[sensorItem].exDataType; // Sensor ID (%16) | EX Data Type + sensorValue = getSensorValue(sensorItem); + iCount = exDataTypeLen[jetiExSensors[sensorItem].exDataType]; while (iCount > 1) { *p++ = sensorValue; sensorValue = sensorValue >> 8; iCount--; } - *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + *p++ = (sensorValue & 0x9F) | jetiExSensors[sensorItem].decimals; - item = getNextActiveSensor(item); + item++; - if (startItem >= item) { + if (item > activeSensors) { + item = 0; break; } - if ((p - &exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType] + 1 >= EXTEL_MAX_PAYLOAD) { + sensorItem = activeSensorList[item]; + if ((p - &exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[sensorItem].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); @@ -341,8 +334,8 @@ 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 + if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) { // to prevent timing issues from request to answer - max. 4ms @@ -382,12 +375,12 @@ void handleJetiExBusTelemetry(void) uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) { static uint8_t sensorDescriptionCounter = 0xFF; - static uint8_t requestLoop = 0xFF; + static uint8_t requestLoop = 0; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; - if (requestLoop) { + if (requestLoop < 0xFF) { while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) { - if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter) || (jetiExSensors[sensorDescriptionCounter].exDataType == EX_TYPE_DES)) { + if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter)) { break; } } @@ -397,11 +390,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); - requestLoop--; - if (requestLoop == 0){ - item = firstActiveSensor; - } + requestLoop++; } else { + if(item >= activeSensors) { + item = 0; + } item = createExTelemetryValueMessage(jetiExTelemetryFrame, item); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); } @@ -411,7 +404,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) return item; } + #endif // TELEMETRY #endif // SERIAL_RX -