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

Merge remote-tracking branch 'refs/remotes/origin/jeti-ex-telemetry'

Browse source
This commit is contained in:
Thomas Miric 2018-01-26 15:07:34 +01:00
commit 694fd135ae
1 changed files with 81 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -114,7 +114,6 @@ typedef struct exBusSensor_s {
// list of telemetry messages // list of telemetry messages
// after every 15 sensors a new header has to be inserted (e.g. "BF D2") // after every 15 sensors a new header has to be inserted (e.g. "BF D2")
exBusSensor_t jetiExSensors[] = { exBusSensor_t jetiExSensors[] = {
{ "BF D1", "", EX_TYPE_DES, EX_TYPE_NONE, EX_TYPE_SENSOR_ENABLED}, // device descripton { "BF D1", "", EX_TYPE_DES, EX_TYPE_NONE, EX_TYPE_SENSOR_ENABLED}, // device descripton
{ "Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}, { "Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
@ -124,30 +123,7 @@ exBusSensor_t jetiExSensors[] = {
{ "Power", "W", EX_TYPE_22b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}, { "Power", "W", EX_TYPE_22b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}, { "Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "Pitch angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}, { "Pitch angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}, { "Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}
/*{ "free", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_22b, DECIMAL_MASK(0), EX_TYPE_SENSOR_DISABLED},
{ "BF D2", "", EX_TYPE_DES, EX_TYPE_NONE, EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED},
{ "free", "", EX_TYPE_14b, DECIMAL_MASK(1), EX_TYPE_SENSOR_DISABLED}*/
}; };
// 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
@ -182,12 +158,12 @@ uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen)
} }
return(crc); return(crc);
} }
/*
-----------------------------------------------
Jeti Ex Bus Telemetry
-----------------------------------------------
*/
/*
* -----------------------------------------------
* Jeti Ex Bus Telemetry
* -----------------------------------------------
*/
void initJetiExBusTelemetry(void) void initJetiExBusTelemetry(void)
{ {
// Init Ex Bus Frame header // Init Ex Bus Frame header
@ -195,7 +171,6 @@ void initJetiExBusTelemetry(void)
jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01;
jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A; // Ex Telemetry jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A; // Ex Telemetry
// 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
@ -205,21 +180,27 @@ 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
// Check which sensors are available // Check which sensors are available
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) jetiExSensors[EX_VOLTAGE].parameter = EX_TYPE_SENSOR_ENABLED; jetiExSensors[EX_VOLTAGE].parameter = EX_TYPE_SENSOR_ENABLED;
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) jetiExSensors[EX_CURRENT].parameter = EX_TYPE_SENSOR_ENABLED; }
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
jetiExSensors[EX_CURRENT].parameter = EX_TYPE_SENSOR_ENABLED;
}
if ((batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_NONE)) { if ((batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_NONE)) {
jetiExSensors[EX_POWER].parameter = EX_TYPE_SENSOR_ENABLED; jetiExSensors[EX_POWER].parameter = EX_TYPE_SENSOR_ENABLED;
jetiExSensors[EX_CAPACITY].parameter = EX_TYPE_SENSOR_ENABLED; jetiExSensors[EX_CAPACITY].parameter = EX_TYPE_SENSOR_ENABLED;
} }
if (sensors(SENSOR_BARO)) jetiExSensors[EX_ALTITUDE].parameter = EX_TYPE_SENSOR_ENABLED; if (sensors(SENSOR_BARO)) {
jetiExSensors[EX_ALTITUDE].parameter = EX_TYPE_SENSOR_ENABLED;
}
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
jetiExSensors[EX_ROLL_ANGLE].parameter = EX_TYPE_SENSOR_ENABLED; jetiExSensors[EX_ROLL_ANGLE].parameter = EX_TYPE_SENSOR_ENABLED;
jetiExSensors[EX_PITCH_ANGLE].parameter = EX_TYPE_SENSOR_ENABLED; jetiExSensors[EX_PITCH_ANGLE].parameter = EX_TYPE_SENSOR_ENABLED;
} }
if (sensors(SENSOR_MAG)) jetiExSensors[EX_HEADING].parameter = EX_TYPE_SENSOR_ENABLED; if (sensors(SENSOR_MAG)) {
jetiExSensors[EX_HEADING].parameter = EX_TYPE_SENSOR_ENABLED;
}
for (uint8_t item = 0; item < JETI_EX_SENSOR_COUNT; item++ ) { for (uint8_t item = 0; item < JETI_EX_SENSOR_COUNT; item++ ) {
if (jetiExSensors[item].parameter == EX_TYPE_SENSOR_ENABLED) { if (jetiExSensors[item].parameter == EX_TYPE_SENSOR_ENABLED) {
@ -229,8 +210,7 @@ void initJetiExBusTelemetry(void)
} }
} }
void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
{ {
uint8_t labelLength = strlen(sensor->label); uint8_t labelLength = strlen(sensor->label);
uint8_t unitLength = strlen(sensor->unit); uint8_t unitLength = strlen(sensor->unit);
@ -246,8 +226,8 @@ 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]); exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]);
} }
int32_t getSensorValue(uint8_t sensor)
int32_t getSensorValue(uint8_t sensor) { {
switch(sensor) { switch(sensor) {
case EX_VOLTAGE: case EX_VOLTAGE:
return (getBatteryVoltageLatest()); return (getBatteryVoltageLatest());
@ -266,7 +246,7 @@ int32_t getSensorValue(uint8_t sensor) {
break; break;
case EX_POWER: case EX_POWER:
return (getBatteryVoltageLatest()*getAmperageLatest()/100); return (getBatteryVoltageLatest() * getAmperageLatest()/100);
break; break;
case EX_ROLL_ANGLE: case EX_ROLL_ANGLE:
@ -281,14 +261,15 @@ int32_t getSensorValue(uint8_t sensor) {
return (attitude.values.yaw); return (attitude.values.yaw);
break; break;
default: return -1; default:
return -1;
} }
} }
uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t item) uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item)
{ {
uint8_t sensorItem = activeSensorList[item]; uint8_t sensorItem = activeSensorList[item];
uint8_t sensorItemStart = sensorItem; uint8_t sensorItemMaxGroup = sensorItem | 0xF0;
uint8_t iCount; uint8_t iCount;
uint8_t messageSize; uint8_t messageSize;
uint32_t sensorValue; uint32_t sensorValue;
@ -297,7 +278,7 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t item)
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 (sensorItem <= (sensorItemStart | 0x0F)) { while (sensorItem <= sensorItemMaxGroup) {
*p++ = ((sensorItem & 0x0F) << 4) | jetiExSensors[sensorItem].exDataType; // Sensor ID (%16) | EX Data Type *p++ = ((sensorItem & 0x0F) << 4) | jetiExSensors[sensorItem].exDataType; // Sensor ID (%16) | EX Data Type
sensorValue = getSensorValue(sensorItem); sensorValue = getSensorValue(sensorItem);
@ -311,10 +292,15 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t item)
item++; item++;
if (item > activeSensors) break; if (item > activeSensors) {
item = 0;
break;
}
sensorItem = activeSensorList[item]; sensorItem = activeSensorList[item];
if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[sensorItem].exDataType]) + 1) break; if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[sensorItem].exDataType]) + 1) {
break;
}
} }
messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID])); messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID]));
@ -324,7 +310,6 @@ uint8_t createExTelemetrieValueMessage(uint8_t *exMessage, uint8_t item)
return item; // return the next item return item; // return the next item
} }
void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID) void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID)
{ {
uint16_t crc16; uint16_t crc16;
@ -343,7 +328,6 @@ void checkJetiExBusTelemetryState(void)
return; return;
} }
void handleJetiExBusTelemetry(void) void handleJetiExBusTelemetry(void)
{ {
static uint16_t framesLost = 0; // only for debug static uint16_t framesLost = 0; // only for debug
@ -387,8 +371,6 @@ 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;
@ -396,20 +378,23 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
if (requestLoop < 0xFF) { if (requestLoop < 0xFF) {
while(1) { while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) {
sensorDescriptionCounter++; if (jetiExSensors[sensorDescriptionCounter].parameter >= EX_TYPE_SENSOR_ENABLED) {
if(jetiExSensors[sensorDescriptionCounter].parameter >= EX_TYPE_SENSOR_ENABLED) break;
if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) {
sensorDescriptionCounter = 0;
break; break;
} }
} }
createExTelemetrieTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) {
sensorDescriptionCounter = 0;
}
createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]);
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
requestLoop++; requestLoop++;
} else { } else {
if(item == activeSensors) item = 0; if(item >= activeSensors) {
item = createExTelemetrieValueMessage(jetiExTelemetryFrame, item); item = 0;
}
item = createExTelemetryValueMessage(jetiExTelemetryFrame, item);
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
} }
@ -421,3 +406,4 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
#endif // TELEMETRY #endif // TELEMETRY
#endif // SERIAL_RX #endif // SERIAL_RX