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.
This commit is contained in:
Thomas
Michael Keller
parent
f992b839c1
commit
6561251446
|
|
@ -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_14b = 1, // int14_t Data type 14b (-8191 ¸8191)
|
||||
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_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_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)
|
||||
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_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")
|
||||
exBusSensor_t jetiExSensors[] = {
|
||||
{ "CF-Dev 1.12 S1", "", 0, 0, 0 }, // device descripton
|
||||
{ "Voltage", "V", 0, EX_TYPE_14b, DECIMAL_MASK(1) },
|
||||
{ "Current", "A", 0, EX_TYPE_14b, DECIMAL_MASK(2) },
|
||||
{ "Altitude", "m", 0, EX_TYPE_14b, DECIMAL_MASK(1) },
|
||||
{ "Capacity", "mAh", 0, EX_TYPE_22b, DECIMAL_MASK(0) },
|
||||
{ "frames lost", " ", 0, EX_TYPE_22b, DECIMAL_MASK(0) }, // for debug only
|
||||
{ "time Diff", "us", 0, EX_TYPE_14b, DECIMAL_MASK(0) } // for debug only
|
||||
// after every 15 sensors a new header has to be inserted (e.g. "BF D2")
|
||||
const exBusSensor_t jetiExSensors[] = {
|
||||
{"BF D1", "", EX_TYPE_DES, 0 }, // device descripton
|
||||
{"Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1)},
|
||||
{"Current", "A", EX_TYPE_14b, DECIMAL_MASK(2)},
|
||||
{"Altitude", "m", EX_TYPE_14b, DECIMAL_MASK(2)},
|
||||
{"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)},
|
||||
{"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)}
|
||||
};
|
||||
|
||||
// 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_TIME_DIFF // for debug only
|
||||
EX_POWER,
|
||||
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)) {
|
||||
*p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType; // Sensor ID (%16) | EX Data Type
|
||||
while (item < sensorItemMaxGroup) {
|
||||
*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++;
|
||||
if (item > JETI_EX_SENSOR_COUNT)
|
||||
break;
|
||||
if (EXTEL_MAX_PAYLOAD <= ((p-&exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType]) + 1)
|
||||
break;
|
||||
}
|
||||
item = getNextActiveSensor(item);
|
||||
|
||||
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 sensorValueCounter = 1;
|
||||
static uint8_t requestLoop = 0;
|
||||
static uint8_t sensorDescriptionCounter = 0xFF;
|
||||
static uint8_t requestLoop = 0xFF;
|
||||
uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
|
||||
|
||||
if (requestLoop == 100) { //every nth request send the name of a value
|
||||
if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT )
|
||||
if (requestLoop) {
|
||||
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 = 0;
|
||||
sensorDescriptionCounter++;
|
||||
requestLoop--;
|
||||
if (requestLoop == 0){
|
||||
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++) {
|
||||
serialWrite(jetiExBusPort, jetiExBusTelemetryFrame[iCount]);
|
||||
}
|
||||
serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]);
|
||||
jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED;
|
||||
requestLoop++;
|
||||
|
||||
return item;
|
||||
}
|
||||
#endif // TELEMETRY
|
||||
#endif // SERIAL_RX
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue