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

Telemetry bugfixes

Browse Source
This commit is contained in:
bsongis
2014-05-15 21:08:04 +02:00
parent b376192b61
commit 6b3c4c8cfe
5 changed files with 94 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/targets/common_avr/telemetry_driver.cpp
+2 -2
View File
@@ -50,7 +50,7 @@ void telemetryEnableRx(void)
}
void processSerialData(uint8_t data);
extern uint8_t numPktBytes; // TODO not driver, change name
extern uint8_t frskyRxBufferCount; // TODO not driver, change name
ISR(USART0_RX_vect)
{
@@ -95,7 +95,7 @@ ISR(USART0_RX_vect)
if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
// discard buffer and start fresh on any comms error
numPktBytes = 0;
frskyRxBufferCount = 0;
}
else {
processSerialData(data);
radio/src/telemetry/frsky.cpp
+79 -78
View File
@@ -63,10 +63,39 @@ uint8_t telemetryState = TELEMETRY_INIT;
Fifo<512> telemetryFifo; // TODO should be in the driver
#endif
uint8_t numPktBytes = 0; // TODO rename
uint8_t frskyRxBufferCount = 0;
FrskyData frskyData;
#if defined(CPUARM)
enum TelemetryProtocol
{
PROTOCOL_NONE,
PROTOCOL_FRSKY_D,
PROTOCOL_FRSKY_SPORT,
};
#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D)
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT)
TelemetryProtocol telemetryProtocol = PROTOCOL_NONE;
TelemetryProtocol getTelemetryProtocol()
{
#if defined(PCBTARANIS)
if (IS_MODULE_XJT(0) || IS_MODULE_XJT(1))
return PROTOCOL_FRSKY_SPORT;
#elif defined(PCBSKY9X)
if (IS_MODULE_XJT())
return PROTOCOL_FRSKY_SPORT;
#endif
return PROTOCOL_FRSKY_D;
}
#else
#define IS_FRSKY_D_PROTOCOL() (true)
#define IS_FRSKY_SPORT_PROTOCOL() (false)
#endif
void FrskyValueWithMin::set(uint8_t value)
{
if (this->value == 0) {
@@ -75,7 +104,7 @@ void FrskyValueWithMin::set(uint8_t value)
else {
sum += value;
if (link_counter == 0) {
this->value = sum / 8;
this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING);
sum = 0;
}
}
@@ -96,29 +125,6 @@ void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit)
}
}
#if defined(CPUARM)
enum TelemetryProtocol
{
PROTOCOL_NONE,
PROTOCOL_FRSKY_D,
PROTOCOL_FRSKY_SPORT,
};
TelemetryProtocol telemetryProtocol = PROTOCOL_NONE;
TelemetryProtocol getTelemetryProtocol()
{
#if defined(PCBTARANIS)
if (IS_MODULE_XJT(0) || IS_MODULE_XJT(1))
return PROTOCOL_FRSKY_SPORT;
#elif defined(PCBSKY9X)
if (IS_MODULE_XJT())
return PROTOCOL_FRSKY_SPORT;
#endif
return PROTOCOL_FRSKY_D;
}
#endif
#if defined(CPUARM)
inline bool alarmRaised(uint8_t channel, uint8_t idx)
{
@@ -150,16 +156,14 @@ NOINLINE void processSerialData(uint8_t data)
{
case STATE_DATA_START:
if (data == START_STOP) {
#if defined(FRSKY_SPORT)
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
dataState = STATE_DATA_IN_FRAME ;
numPktBytes = 0;
frskyRxBufferCount = 0;
}
#endif
}
else {
if (numPktBytes < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[numPktBytes++] = data;
if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[frskyRxBufferCount++] = data;
}
dataState = STATE_DATA_IN_FRAME;
}
@@ -170,38 +174,32 @@ NOINLINE void processSerialData(uint8_t data)
dataState = STATE_DATA_XOR; // XOR next byte
}
else if (data == START_STOP) {
#if defined(FRSKY_SPORT)
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
dataState = STATE_DATA_IN_FRAME ;
numPktBytes = 0;
frskyRxBufferCount = 0;
}
else {
// end of frame detected
frskyDProcessPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
}
#else
// end of frame detected
frskyDProcessPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
#endif
break;
}
else if (numPktBytes < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[numPktBytes++] = data;
else if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[frskyRxBufferCount++] = data;
}
break;
case STATE_DATA_XOR:
if (numPktBytes < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[numPktBytes++] = data ^ STUFF_MASK;
if (frskyRxBufferCount < FRSKY_RX_PACKET_SIZE) {
frskyRxBuffer[frskyRxBufferCount++] = data ^ STUFF_MASK;
}
dataState = STATE_DATA_IN_FRAME;
break;
case STATE_DATA_IDLE:
if (data == START_STOP) {
numPktBytes = 0;
frskyRxBufferCount = 0;
dataState = STATE_DATA_START;
}
#if defined(TELEMETREZ)
@@ -242,18 +240,18 @@ NOINLINE void processSerialData(uint8_t data)
#endif
} // switch
#if defined(FRSKY_SPORT)
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT && numPktBytes >= FRSKY_SPORT_PACKET_SIZE) {
if (IS_FRSKY_SPORT_PROTOCOL() && frskyRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
frskySportProcessPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
}
#endif
}
void telemetryWakeup()
{
#if defined(CPUARM)
if (telemetryProtocol != getTelemetryProtocol()) {
TelemetryProtocol currentProtocol = getTelemetryProtocol();
if (telemetryProtocol != currentProtocol) {
telemetryProtocol = currentProtocol;
telemetryInit();
}
#endif
@@ -284,29 +282,16 @@ void telemetryWakeup()
#endif
#if !defined(PCBTARANIS)
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
static uint8_t frskyTxDelay = 5;
if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
frskyTxDelay = 5; // 50ms
frskyDSendNextAlarm();
if (IS_FRSKY_D_PROTOCOL()) {
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
static uint8_t frskyTxDelay = 5;
if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
frskyTxDelay = 5; // 50ms
frskyDSendNextAlarm();
}
}
#endif
#if !defined(SIMU)
#if defined(WS_HOW_HIGH)
if (frskyUsrStreaming > 0) {
frskyUsrStreaming--;
}
#endif
if (frskyStreaming > 0) {
frskyStreaming--;
}
else {
frskyData.rssi[0].set(0);
frskyData.rssi[1].set(0);
}
#endif
#if defined(VARIO)
if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
@@ -320,15 +305,14 @@ void telemetryWakeup()
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1second */
alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1 second */
if (alarmsCheckStep == 0) {
#if defined(PCBTARANIS)
// TODO not only Taranis!
if ((IS_MODULE_XJT(0) || IS_MODULE_XJT(1)) && frskyData.swr.value > 0x33) {
if (IS_FRSKY_SPORT_PROTOCOL() && frskyData.swr.value > 0x33) {
AUDIO_SWR_RED();
POPUP_WARNING(STR_ANTENNAPROBLEM);
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
#endif
}
@@ -336,31 +320,31 @@ void telemetryWakeup()
if (alarmsCheckStep == 1) {
if (getRssiAlarmValue(1) && frskyData.rssi[0].value < getRssiAlarmValue(1)) {
AUDIO_RSSI_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
else if (getRssiAlarmValue(0) && frskyData.rssi[0].value < getRssiAlarmValue(0)) {
AUDIO_RSSI_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
}
else if (alarmsCheckStep == 2) {
if (alarmRaised(1, 1)) {
AUDIO_A2_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
else if (alarmRaised(1, 0)) {
AUDIO_A2_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
}
else if (alarmsCheckStep == 3) {
if (alarmRaised(0, 1)) {
AUDIO_A1_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
else if (alarmRaised(0, 0)) {
AUDIO_A1_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
}
}
}
@@ -438,12 +422,29 @@ void telemetryInterrupt10ms()
frskyData.hub.maxPower = frskyData.hub.power;
}
}
#if defined(WS_HOW_HIGH)
if (frskyUsrStreaming > 0) {
frskyUsrStreaming--;
}
#endif
if (frskyStreaming > 0) {
frskyStreaming--;
}
else {
frskyData.rssi[0].set(0);
frskyData.rssi[1].set(0);
}
}
void telemetryReset()
{
memclear(&frskyData, sizeof(frskyData));
frskyStreaming = 0; // reset counter only if valid frsky packets are being detected
link_counter = 0;
#if defined(CPUARM)
telemetryState = TELEMETRY_INIT;
#endif
radio/src/telemetry/frsky.h
+8 -5
View File
@@ -39,13 +39,16 @@
#include <inttypes.h>
#define FRSKY_SPORT_BAUDRATE 57600
#define FRSKY_D_BAUDRATE 9600
#define FRSKY_SPORT_BAUDRATE 57600
#define FRSKY_D_BAUDRATE 9600
#define FRSKY_SPORT_PACKET_SIZE 9
#define FRSKY_SPORT_PACKET_SIZE 9
// 20 seconds
#define FRSKY_TIMEOUT10ms 20
#define FRSKY_TIMEOUT10ms 100 // 1 second
#define WSHH_TIMEOUT10ms 60 // 600ms
#define FRSKY_SPORT_AVERAGING 4
#define FRSKY_D_AVERAGING 8
// Global Fr-Sky telemetry data variables
extern uint8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time
radio/src/telemetry/frsky_d.cpp
+3 -3
View File
@@ -276,7 +276,7 @@ void parseTelemHubByte(uint8_t byte)
#if defined(WS_HOW_HIGH)
void parseTelemWSHowHighByte(uint8_t byte)
{
if (frskyUsrStreaming < (FRSKY_TIMEOUT10ms*3 - 10)) {
if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)] = byte;
checkMinMaxAltitude();
}
@@ -285,7 +285,7 @@ void parseTelemWSHowHighByte(uint8_t byte)
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)+1] = byte;
}
// baroAltitude_bp unit here is feet!
frskyUsrStreaming = FRSKY_TIMEOUT10ms*3; // reset counter
frskyUsrStreaming = WSHH_TIMEOUT10ms; // reset counter
}
#endif
@@ -296,12 +296,12 @@ void frskyDProcessPacket(uint8_t *packet)
{
case LINKPKT: // A1/A2/RSSI values
{
link_counter += 32;
frskyData.analog[0].set(packet[1], g_model.frsky.channels[0].type);
frskyData.analog[1].set(packet[2], g_model.frsky.channels[1].type);
frskyData.rssi[0].set(packet[3]);
frskyData.rssi[1].set(packet[4] / 2);
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
link_counter += 256 / FRSKY_D_AVERAGING;
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource < 2)
radio/src/telemetry/frsky_sport.cpp
+2 -2
View File
@@ -304,7 +304,8 @@ void frskySportProcessPacket(uint8_t *packet)
case DATA_FRAME:
if (appId == RSSI_ID) {
link_counter += 32;
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
link_counter += 256 / FRSKY_SPORT_AVERAGING;
frskyData.rssi[0].set(SPORT_DATA_U8(packet));
}
if (appId == SWR_ID) {
@@ -502,6 +503,5 @@ void frskySportProcessPacket(uint8_t *packet)
}
}
break;
}
}