Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Revert "rebase"

Browse source 
This reverts commit be89a6428a.
This commit is contained in:
Thomas Miric 2018-01-31 23:54:43 +01:00
parent be89a6428a
commit b16c9e9adf
1 changed files with 42 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

91
src/main/telemetry/jetiexbus.c
View file

@ -40,6 +40,7 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "io/serial.h" #include "io/serial.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/jetiexbus.h" #include "rx/jetiexbus.h"
@ -54,11 +55,12 @@
#define EXTEL_SYNC_LEN 1 #define EXTEL_SYNC_LEN 1
#define EXTEL_CRC_LEN 1 #define EXTEL_CRC_LEN 1
#define EXTEL_HEADER_LEN 6 #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_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN)
#define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD) #define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD)
#define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN) #define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN)
enum exTelHeader_e { enum exTelHeader_e {
EXTEL_HEADER_SYNC = 0, EXTEL_HEADER_SYNC = 0,
EXTEL_HEADER_TYPE_LEN, EXTEL_HEADER_TYPE_LEN,
@ -80,6 +82,7 @@ enum {
}; };
enum exDataType_e { enum exDataType_e {
EX_TYPE_NONE = 0,
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)
@ -118,8 +121,7 @@ const exBusSensor_t jetiExSensors[] = {
{"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)},
{"Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, {"Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)},
{"Pitch 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)}, {"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 // 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_POWER,
EX_ROLL_ANGLE, EX_ROLL_ANGLE,
EX_PITCH_ANGLE, EX_PITCH_ANGLE,
EX_HEADING, EX_HEADING
EX_VARIO
}; };
#define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors)) #define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors))
static uint8_t jetiExBusTelemetryFrame[40]; static uint8_t jetiExBusTelemetryFrame[40];
static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX; static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX;
static uint8_t firstActiveSensor = 0; static uint8_t activeSensorList[JETI_EX_SENSOR_COUNT];
static uint32_t exSensorEnabled = 0; static uint8_t activeSensors;
static uint32_t exSensorEnabled;
static uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item); 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 // Jeti Ex Telemetry CRC calculations for a frame
uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen)
{ {
uint8_t crc=0; uint8_t crc=0;
for (uint8_t mlen = 0; mlen < msgLen; mlen++) { for (uint8_t mlen = 0; mlen < msgLen; mlen++) {
crc ^= pt[mlen]; crc ^= pt[mlen];
crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18)); crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18));
@ -170,7 +172,6 @@ void initJetiExBusTelemetry(void)
// Init Ex Telemetry header // Init Ex Telemetry header
uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F; // Startbyte jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F; // Startbyte
jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E; // Serial Number 4 Byte jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E; // Serial Number 4 Byte
jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4; jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4;
@ -178,7 +179,8 @@ void initJetiExBusTelemetry(void)
jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00;
jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00
//exSensorEnabled = 0x3fe;
bitArraySet(&exSensorEnabled, 0);
// Check which sensors are available // Check which sensors are available
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
bitArraySet(&exSensorEnabled, EX_VOLTAGE); bitArraySet(&exSensorEnabled, EX_VOLTAGE);
@ -192,7 +194,6 @@ void initJetiExBusTelemetry(void)
} }
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
bitArraySet(&exSensorEnabled, EX_ALTITUDE); bitArraySet(&exSensorEnabled, EX_ALTITUDE);
bitArraySet(&exSensorEnabled, EX_VARIO);
} }
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE); bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
@ -201,7 +202,13 @@ void initJetiExBusTelemetry(void)
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
bitArraySet(&exSensorEnabled, EX_HEADING); 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) 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; return attitude.values.yaw;
break; break;
case EX_VARIO:
return getEstimatedVario();
break;
default: default:
return -1; 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 createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item)
{ {
uint8_t startItem = item; uint8_t sensorItem = activeSensorList[item];
uint8_t sensorItemMaxGroup = (item & 0xF0) + 0x10; uint8_t sensorItemMaxGroup = sensorItem | 0xF0;
uint8_t iCount; uint8_t iCount;
uint8_t messageSize; uint8_t messageSize;
uint32_t sensorValue; uint32_t sensorValue;
exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID
uint8_t *p = &exMessage[EXTEL_HEADER_ID]; uint8_t *p = &exMessage[EXTEL_HEADER_ID];
while (item < sensorItemMaxGroup) { while (sensorItem <= sensorItemMaxGroup) {
*p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type *p++ = ((sensorItem & 0x0F) << 4) | jetiExSensors[sensorItem].exDataType; // Sensor ID (%16) | EX Data Type
sensorValue = getSensorValue(item);
iCount = exDataTypeLen[jetiExSensors[item].exDataType];
sensorValue = getSensorValue(sensorItem);
iCount = exDataTypeLen[jetiExSensors[sensorItem].exDataType];
while (iCount > 1) { while (iCount > 1) {
*p++ = sensorValue; *p++ = sensorValue;
sensorValue = sensorValue >> 8; sensorValue = sensorValue >> 8;
iCount--; 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; 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; break;
} }
} }
messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID])); messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID]));
exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize; exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize;
exMessage[messageSize + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], 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 uint16_t framesLost = 0; // only for debug
static uint8_t item = 0; static uint8_t item = 0;
uint32_t timeDiff; 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) { if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) {
// to prevent timing issues from request to answer - max. 4ms // 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) uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
{ {
static uint8_t sensorDescriptionCounter = 0xFF; static uint8_t sensorDescriptionCounter = 0xFF;
static uint8_t requestLoop = 0xFF; static uint8_t requestLoop = 0;
uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
if (requestLoop) { if (requestLoop < 0xFF) {
while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) { while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) {
if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter) || (jetiExSensors[sensorDescriptionCounter].exDataType == EX_TYPE_DES)) { if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter)) {
break; break;
} }
} }
@ -397,11 +390,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]);
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
requestLoop--; requestLoop++;
if (requestLoop == 0){
item = firstActiveSensor;
}
} else { } else {
if(item >= activeSensors) {
item = 0;
}
item = createExTelemetryValueMessage(jetiExTelemetryFrame, item); item = createExTelemetryValueMessage(jetiExTelemetryFrame, item);
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
} }
@ -411,7 +404,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
return item; return item;
} }
#endif // TELEMETRY #endif // TELEMETRY
#endif // SERIAL_RX #endif // SERIAL_RX