piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

S.PORT Push reworking

Browse Source
This commit is contained in:
Bertrand Songis
2019-03-28 18:43:07 +01:00
parent 0feceff635
commit 95fe5c59bb
12 changed files with 99 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/frsky.cpp
-70
View File
@@ -20,76 +20,6 @@
#include "opentx.h"
bool checkPXX2PacketCRC(const uint8_t * packet)
{
// TODO ...
return true;
// TRACE("checkPXX2PacketCRC(): checksum error ");
// DUMP(packet, FRSKY_SPORT_PACKET_SIZE);
}
void createFrSkyPXX2Sensor(uint16_t type, uint8_t value){
uint8_t buffer[5] = {0x18, 0x10, 0, 0, 0};
buffer[2] = type & 0xF;
buffer[3] = type >> 8;
buffer[4] = value;
sportProcessTelemetryPacketWithoutCrc(buffer);
}
void processFrskyPXX2Data(uint8_t data)
{
static uint8_t dataState = STATE_DATA_IDLE;
switch (dataState) {
case STATE_DATA_IDLE:
if (data == START_STOP) {
telemetryRxBufferCount = 0;
dataState = STATE_DATA_START;
}
break;
case STATE_DATA_START:
if (telemetryRxBufferCount == 0 && data > 0x0C) {
// wrong length
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) {
if (checkPXX2PacketCRC(telemetryRxBuffer)) {
R9ModuleStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
uint8_t R9Region = (telemetryRxBuffer[3] & 0x30) >> 4;
if (R9Region != 0x04) {
g_model.moduleData[EXTERNAL_MODULE].r9m.region = R9Region;
}
if (telemetryRxBuffer[2] & 0x80) {
createFrSkyPXX2Sensor(RSSI_ID, telemetryRxBuffer[2] & 0x7f);
}
else {
createFrSkyPXX2Sensor(BATT_ID, telemetryRxBuffer[2] & 0x7f);
}
if((telemetryRxBuffer[3] & 0x04) == 0) {
createFrSkyPXX2Sensor(R9_PWR_ID, (telemetryRxBuffer[3] & 0x03));
}
if (telemetryRxBuffer[0] == 0x0C) {
sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 5 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */);
}
}
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
}
else {
// overflow guard
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
break;
}
}
void processFrskyTelemetryData(uint8_t data)
{
#if defined(PCBSKY9X) && defined(BLUETOOTH)
radio/src/telemetry/frsky.h
+1 -2
View File
@@ -284,7 +284,7 @@ void frskyDProcessPacket(const uint8_t *packet);
// FrSky S.PORT Telemetry Protocol
void sportProcessTelemetryPacket(const uint8_t * packet);
void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet);
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet);
void telemetryWakeup();
void telemetryReset();
@@ -315,6 +315,5 @@ extern TelemetryData telemetryData;
bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
void processFrskyTelemetryData(uint8_t data);
void processFrskyPXX2Data(uint8_t data);
#endif // _FRSKY_H_
radio/src/telemetry/frsky_pxx2.cpp
+4 -14
View File
@@ -166,7 +166,10 @@ void processBindFrame(uint8_t module, uint8_t * frame)
void processTelemetryFrame(uint8_t module, uint8_t * frame)
{
sportProcessTelemetryPacketWithoutCrc(&frame[4]);
TelemetryEndpoint origin;
origin.module = module;
origin.rxUid = frame[3] & 0x0F;
sportProcessTelemetryPacketWithoutCrc(origin.value, &frame[4]);
}
void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame)
@@ -260,16 +263,3 @@ void processPXX2Frame(uint8_t module, uint8_t *frame)
break;
}
}
void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet)
{
// Flag0
outputTelemetryBuffer.push(rx_uid);
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
uint8_t byte = packet->raw[i];
outputTelemetryBuffer.push(byte);
}
outputTelemetryBuffer.setDestination(module);
}
radio/src/telemetry/frsky_sport.cpp
+42 -38
View File
@@ -147,19 +147,23 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
return;
}
sportProcessTelemetryPacketWithoutCrc(packet);
sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
}
void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
{
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
uint16_t id = *((uint16_t *)(packet+2));
uint16_t dataId = *((uint16_t *)(packet+2));
uint32_t data = SPORT_DATA_S32(packet);
if (primId == DATA_FRAME) {
if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) {
outputTelemetryBuffer.sport.physicalId = packet[0];
outputTelemetryBuffer.destination.value = origin;
}
uint8_t instance = physicalId + 1;
if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
data = SPORT_DATA_U8(packet);
if (data == 0)
@@ -167,17 +171,17 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
else
telemetryData.rssi.set(data);
}
else if (id == R9_PWR_ID) {
else if (dataId == R9_PWR_ID) {
uint32_t r9pwr[] = {100, 200, 500, 1000};
data = r9pwr[SPORT_DATA_U8(packet) & 0x03];
}
else if (id == XJT_VERSION_ID) {
else if (dataId == XJT_VERSION_ID) {
telemetryData.xjtVersion = HUB_DATA_U16(packet);
if (!IS_RAS_VALUE_VALID()) {
telemetryData.swr.set(0x00);
}
}
else if (id == RAS_ID) {
else if (dataId == RAS_ID) {
if (IS_RAS_VALUE_VALID())
telemetryData.swr.set(SPORT_DATA_U8(packet));
else
@@ -185,33 +189,33 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
}
if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
if ((id >> 8) == 0) {
if ((dataId >> 8) == 0) {
// The old FrSky IDs
processHubPacket(id, HUB_DATA_U16(packet));
processHubPacket(dataId, HUB_DATA_U16(packet));
}
else if (!IS_HIDDEN_TELEMETRY_VALUE(id)) {
if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == RAS_ID) {
else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) {
if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) {
data = SPORT_DATA_U8(packet);
}
if (id >= GPS_LONG_LATI_FIRST_ID && id <= GPS_LONG_LATI_LAST_ID) {
if (dataId >= GPS_LONG_LATI_FIRST_ID && dataId <= GPS_LONG_LATI_LAST_ID) {
int32_t value = (data & 0x3fffffff);
if (data & (1 << 30))
value = -value;
value = (value * 5) / 3; // min/10000 => deg/1000000
if (data & (1 << 31))
sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LONGITUDE);
sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LONGITUDE);
else
sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LATITUDE);
sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LATITUDE);
}
else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= RBOX_BATT1_FIRST_ID && dataId <= RBOX_BATT2_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= RBOX_CNSP_FIRST_ID && id <= RBOX_CNSP_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= RBOX_CNSP_FIRST_ID && dataId <= RBOX_CNSP_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= RBOX_STATE_FIRST_ID && id <= RBOX_STATE_LAST_ID) {
else if (dataId >= RBOX_STATE_FIRST_ID && dataId <= RBOX_STATE_LAST_ID) {
bool static isRB10 = false;
uint16_t newServosState;
@@ -233,31 +237,31 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
}
servosState = newServosState;
rboxState = newRboxState;
sportProcessTelemetryPacket(id, 0, instance, servosState);
sportProcessTelemetryPacket(id, 1, instance, rboxState);
sportProcessTelemetryPacket(dataId, 0, instance, servosState);
sportProcessTelemetryPacket(dataId, 1, instance, rboxState);
}
else if (id >= ESC_POWER_FIRST_ID && id <= ESC_POWER_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= ESC_POWER_FIRST_ID && dataId <= ESC_POWER_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= ESC_RPM_CONS_FIRST_ID && id <= ESC_RPM_CONS_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, 100 * (data & 0xffff));
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= ESC_RPM_CONS_FIRST_ID && dataId <= ESC_RPM_CONS_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, 100 * (data & 0xffff));
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= ESC_TEMPERATURE_FIRST_ID && id <= ESC_TEMPERATURE_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0x00ff);
else if (dataId >= ESC_TEMPERATURE_FIRST_ID && dataId <= ESC_TEMPERATURE_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0x00ff);
}
else if (id >= SBEC_POWER_FIRST_ID && id <= SBEC_POWER_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, (data & 0xffff) / 10);
sportProcessTelemetryPacket(id, 1, instance, (data >> 16) / 10);
else if (dataId >= SBEC_POWER_FIRST_ID && dataId <= SBEC_POWER_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, (data & 0xffff) / 10);
sportProcessTelemetryPacket(dataId, 1, instance, (data >> 16) / 10);
}
else if (id >= DIY_STREAM_FIRST_ID && id <= DIY_STREAM_LAST_ID) {
else if (dataId >= DIY_STREAM_FIRST_ID && dataId <= DIY_STREAM_LAST_ID) {
#if defined(LUA)
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
SportTelemetryPacket luaPacket;
luaPacket.physicalId = physicalId;
luaPacket.primId = primId;
luaPacket.dataId = id;
luaPacket.dataId = dataId;
luaPacket.value = data;
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
luaInputTelemetryFifo->push(luaPacket.raw[i]);
@@ -266,7 +270,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
#endif
}
else {
sportProcessTelemetryPacket(id, 0, instance, data);
sportProcessTelemetryPacket(dataId, 0, instance, data);
}
}
}
@@ -277,7 +281,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
SportTelemetryPacket luaPacket;
luaPacket.physicalId = physicalId;
luaPacket.primId = primId;
luaPacket.dataId = id;
luaPacket.dataId = dataId;
luaPacket.value = data;
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
luaInputTelemetryFifo->push(luaPacket.raw[i]);
radio/src/telemetry/telemetry.cpp
-5
View File
@@ -63,11 +63,6 @@ void processTelemetryData(uint8_t data)
}
#endif
if (telemetryProtocol == PROTOCOL_TELEMETRY_PXX2) {
processFrskyPXX2Data(data);
return;
}
processFrskyTelemetryData(data);
}
radio/src/telemetry/telemetry.h
+23 -19
View File
@@ -155,7 +155,17 @@ void logTelemetryWriteByte(uint8_t data);
#define LOG_TELEMETRY_WRITE_BYTE(data)
#endif
#define TELEMETRY_OUTPUT_FIFO_SIZE 20
#define TELEMETRY_ENDPOINT_ANY 0xFF
#define TELEMETRY_ENDPOINT_NONE 0xFE
#define TELEMETRY_ENDPOINT_SPORT 0xFD
union TelemetryEndpoint {
PACK(struct {
uint8_t module:4;
uint8_t rxUid:4;
});
uint8_t value;
};
class OutputTelemetryBuffer {
public:
@@ -163,29 +173,24 @@ class OutputTelemetryBuffer {
reset();
}
void push(uint8_t byte) {
data[size++] = byte;
}
void setTrigger(uint8_t byte) {
trigger = byte;
}
void setDestination(uint8_t module) {
destination = module;
void setDestination(uint8_t value) {
destination.value = value;
}
void reset() {
size = 0;
trigger = 0x7E;
destination = 0xFF;
destination.value = TELEMETRY_ENDPOINT_NONE;
}
bool isAvailable() {
return destination.value == TELEMETRY_ENDPOINT_NONE;
}
public:
uint8_t data[TELEMETRY_OUTPUT_FIFO_SIZE];
uint8_t size;
uint8_t trigger;
uint8_t destination;
union {
SportTelemetryPacket sport;
uint8_t data[16];
};
TelemetryEndpoint destination;
};
extern OutputTelemetryBuffer outputTelemetryBuffer __DMA;
@@ -202,6 +207,5 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
#endif
void processPXX2Frame(uint8_t module, uint8_t *frame);
void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet);
#endif // _TELEMETRY_H_