diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h
index 661b8eb..dfbefaa 100644
--- a/remote/include/FrSkyD_cc2500.h
+++ b/remote/include/FrSkyD_cc2500.h
@@ -29,8 +29,18 @@ enum {
};
#define MAX_PKT 29
+extern uint8_t rx_tx_addr[5];
extern uint8_t pkt[MAX_PKT];//telemetry receiving packets
-
+extern uint8_t pktt[MAX_PKT];//telemetry receiving packets
+extern uint8_t v_lipo1;
+extern uint8_t v_lipo2;
+extern uint8_t RX_RSSI;
+extern uint8_t TX_RSSI;
+extern uint8_t RX_LQI;
+extern uint8_t TX_LQI;
+extern uint8_t telemetry_link;
+extern uint8_t telemetry_lost;
+extern uint8_t telemetry_counter;
extern uint8_t freq_offset;
extern uint16_t state;
diff --git a/remote/include/Multiprotocol.h b/remote/include/Multiprotocol.h
index 8fc35d4..aef0d0e 100644
--- a/remote/include/Multiprotocol.h
+++ b/remote/include/Multiprotocol.h
@@ -30,7 +30,6 @@ extern uint8_t crc8;
extern uint8_t packet_count;
extern uint8_t binding_idx;
extern uint8_t packet[40];
-extern uint8_t rx_tx_addr[5];
extern uint8_t option;
extern uint16_t state;
extern uint8_t phase;
diff --git a/remote/include/state.h b/remote/include/state.h
index b8464bd..23ddd41 100644
--- a/remote/include/state.h
+++ b/remote/include/state.h
@@ -7,9 +7,7 @@
extern LiquidCrystal_I2C lcd;
enum lcd_special_chars {
- battery_char = 0,
- battery_char_1 = 0,
- battery_char_2 = 1,
+ battery_char= 0,
rssiantenna= 3,
rssi_bars = 4,
@@ -66,7 +64,8 @@ private:
unsigned long time_enter;
uint16_t last_time;
- void print_akku(uint8_t akku_quad, uint8_t akku_remote);
+ void print_akku_quad(uint8_t akku_quad);
+ void print_akku_remote(uint8_t akku_remote);
void print_rssi(uint8_t rssi_percent);
void print_time(uint16_t time);
diff --git a/remote/include/telemetry.h b/remote/include/telemetry.h
index 33ce1f7..42230f1 100644
--- a/remote/include/telemetry.h
+++ b/remote/include/telemetry.h
@@ -1,361 +1,18 @@
-/*
- This project is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- Multiprotocol is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with Multiprotocol. If not, see .
- */
-//**************************
-// Telemetry serial code *
-//**************************
-#if defined TELEMETRY
-
-uint8_t RetrySequence ;
-
-#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
- #define MULTI_TIME 500 //in ms
- #define INPUT_SYNC_TIME 100 //in ms
- #define INPUT_ADDITIONAL_DELAY 100 // in 10µs, 100 => 1000 µs
- uint32_t lastMulti = 0;
-#endif // MULTI_TELEMETRY/MULTI_STATUS
-
-#if defined SPORT_TELEMETRY
- #define SPORT_TIME 12000 //12ms
- #define FRSKY_SPORT_PACKET_SIZE 8
- #define FX_BUFFERS 4
- uint32_t last = 0;
- uint8_t sport_counter=0;
- uint8_t RxBt = 0;
- uint8_t sport = 0;
- uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
-
- // Store for out of sequence packet
- uint8_t FrskyxRxTelemetryValidSequence ;
- struct t_fx_rx_frame
- {
- uint8_t valid ;
- uint8_t count ;
- uint8_t payload[6] ;
- } ;
-
- // Store for FrskyX telemetry
- struct t_fx_rx_frame FrskyxRxFrames[4] ;
- uint8_t NextFxFrameToForward ;
- #ifdef SPORT_POLLING
- uint8_t sport_rx_index[28] ;
- uint8_t ukindex ;
- uint8_t kindex ;
- uint8_t TxData[2];
- uint8_t SportIndexPolling;
- uint8_t RxData[16] ;
- volatile uint8_t RxIndex=0 ;
- uint8_t sport_bytes=0;
- uint8_t skipped_id;
- uint8_t rx_counter=0;
- #endif
-#endif // SPORT_TELEMETRY
-
-#if defined HUB_TELEMETRY
- #define USER_MAX_BYTES 6
- uint8_t prev_index;
-#endif // HUB_TELEMETRY
-
-#define START_STOP 0x7e
-#define BYTESTUFF 0x7d
-#define STUFF_MASK 0x20
-#define MAX_PKTX 10
-uint8_t pktx[MAX_PKTX];
-uint8_t indx;
-uint8_t frame[18];
+#ifndef __TELE_METRY_H_
+#define __TELE_METRY_H_
+#include
#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
-static void multi_send_header(uint8_t type, uint8_t len)
-{
- Serial_write('M');
- #ifdef MULTI_TELEMETRY
- Serial_write('P');
- Serial_write(type);
- #else
- (void)type;
- #endif
- Serial_write(len);
-}
-
+static void multi_send_header(uint8_t type, uint8_t len);
#endif
#ifdef MULTI_TELEMETRY
-static void multi_send_frskyhub()
-{
- multi_send_header(MULTI_TELEMETRY_HUB, 9);
- for (uint8_t i = 0; i < 9; i++)
- Serial_write(frame[i]);
-}
+static void multi_send_frskyhub();
#endif
-void frskySendStuffed()
-{
- Serial_write(START_STOP);
- for (uint8_t i = 0; i < 9; i++) {
- if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF)) {
- Serial_write(BYTESTUFF);
- frame[i] ^= STUFF_MASK;
- }
- Serial_write(frame[i]);
- }
- Serial_write(START_STOP);
-}
-
-void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
-{
- uint8_t clen = pkt[0] + 3 ;
- if (len != clen) // wrong length
- return;
- if(pkt[1] == rx_tx_addr[3] &&
- pkt[2] == rx_tx_addr[2] ) {
- telemetry_link|=1; // Telemetry data is available
- TX_RSSI = pkt[len-2];
-
- if(TX_RSSI >=128)
- TX_RSSI -= 128;
- else
- TX_RSSI += 128;
-
- TX_LQI = pkt[len-1]&0x7F;
- for (uint8_t i=3;i0 && pktt[6]<=10) {
- if (protocol==PROTO_FRSKYD) {
- if ( ( pktt[7] & 0x1F ) == (telemetry_counter & 0x1F) ) {
- uint8_t topBit = 0 ;
- if ( telemetry_counter & 0x80 )
- if ( ( telemetry_counter & 0x1F ) != RetrySequence )
- topBit = 0x80 ;
- telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
- } else {
- // incorrect sequence
- RetrySequence = pktt[7] & 0x1F ;
- telemetry_counter |= 0x80 ;
- pktt[6]=0 ; // Discard current packet and wait for retransmit
- }
- }
- } else
- pktt[6]=0; // Discard packet
- //
-#if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
- telemetry_lost=0;
- if (protocol==PROTO_FRSKYX) {
- uint16_t lcrc = frskyX_crc_x(&pkt[3], len-7 ) ;
- if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) ) {
- // Check if in sequence
- if ( (pkt[5] & 0x0F) == 0x08 ) {
- FrX_receive_seq = 0x08 ;
- NextFxFrameToForward = 0 ;
- FrskyxRxFrames[0].valid = 0 ;
- FrskyxRxFrames[1].valid = 0 ;
- FrskyxRxFrames[2].valid = 0 ;
- FrskyxRxFrames[3].valid = 0 ;
- } else if ( (pkt[5] & 0x03) == (FrX_receive_seq & 0x03 ) ) {
- // OK to process
- struct t_fx_rx_frame *p ;
- uint8_t count ;
- p = &FrskyxRxFrames[FrX_receive_seq & 3] ;
- count = pkt[6] ;
- if ( count <= 6 ) {
- p->count = count ;
- for ( uint8_t i = 0 ; i < count ; i += 1 )
- p->payload[i] = pkt[i+7] ;
- }
- else
- p->count = 0 ;
- p->valid = 1 ;
+void frsky_check_telemetry(uint8_t *pkt,uint8_t len);
- FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
-
- if ( FrskyxRxTelemetryValidSequence & 0x80 ) {
- FrX_receive_seq = ( FrskyxRxTelemetryValidSequence + 1 ) & 3 ;
- FrskyxRxTelemetryValidSequence &= 0x7F ;
- }
- } else {
- // Save and request correct packet
- struct t_fx_rx_frame *q ;
- uint8_t count ;
- // pkt[4] RSSI
- // pkt[5] sequence control
- // pkt[6] payload count
- // pkt[7-12] payload
- pktt[6] = 0 ; // Don't process
- if ( (pkt[5] & 0x03) == ( ( FrX_receive_seq +1 ) & 3 ) ) {
- q = &FrskyxRxFrames[(pkt[5] & 0x03)] ;
- count = pkt[6] ;
- if ( count <= 6 ) {
- q->count = count ;
- for ( uint8_t i = 0 ; i < count ; i += 1 ) {
- q->payload[i] = pkt[i+7] ;
- }
- } else
- q->count = 0 ;
- q->valid = 1 ;
-
- FrskyxRxTelemetryValidSequence = 0x80 | ( pkt[5] & 0x03 ) ;
- }
-
- FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ; // Request re-transmission
- }
-
- if (((pktt[5] >> 4) & 0x0f) == 0x08)
- FrX_send_seq = 0 ;
- }
- }
+void print_frskyd_telemetry();
+void init_frskyd_link_telemetry();
#endif
- }
-}
-
-void init_frskyd_link_telemetry()
-{
- telemetry_link=0;
- telemetry_counter=0;
- v_lipo1=0;
- v_lipo2=0;
- RX_RSSI=0;
- TX_RSSI=0;
- RX_LQI=0;
- TX_LQI=0;
-}
-
-#if defined SPORT_TELEMETRY
-/* SPORT details serial
- 100K 8E2 normal-multiprotocol
- -every 12ms-or multiple of 12; %36
- 1 2 3 4 5 6 7 8 9 CRC DESCR
- 7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
- 7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
- 7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
- 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
-
-
- Telemetry frames(RF) SPORT info
- 15 bytes payload
- SPORT frame valid 6+3 bytes
- [00] PKLEN 0E 0E 0E 0E
- [01] TXID1 DD DD DD DD
- [02] TXID2 6D 6D 6D 6D
- [03] CONST 02 02 02 02
- [04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
- [05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
- [06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
- [07] STRM1 00 00 7E 00
- [08] STRM2 00 00 1A 00
- [09] STRM3 00 00 10 00
- [10] STRM4 03 03 03 03
- [11] STRM5 F1 F1 F1 F1
- [12] STRM6 D1 D1 D0 D0
- [13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
- [14] CHKSUM2 --|
- +2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
-
-0x06 0x06 0x06 0x06 0x06
-
-0x7E 0x00 0x03 0x7E 0x00
-0x1A 0x00 0xF1 0x1A 0x00
-0x10 0x00 0xD7 0x10 0x00
-0x03 0x7E 0x00 0x03 0x7E
-0xF1 0x1A 0x00 0xF1 0x1A
-0xD7 0x10 0x00 0xD7 0x10
-
-0xE1 0x1C 0xD0 0xEE 0x33
-0x34 0x0A 0xC3 0x56 0xF3
-
- */
-#if defined SPORT_POLLING || defined MULTI_TELEMETRY
-const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
- 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
- 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
- 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
- 0x98, 0x39, 0xBA, 0x1B } ;
-#endif
-
-
-
-#endif
-
-/**************************/
-/**************************/
-/** Serial TX routines **/
-/**************************/
-/**************************/
-
-// Routines for bit-bashed serial output
-
-void Serial_write( uint8_t byte )
-{
- uint8_t temp ;
- uint8_t temp1 ;
- uint8_t byteLo ;
-
- #ifdef INVERT_SERIAL
- byte = ~byte ;
- #endif
-
- byteLo = byte ;
- byteLo >>= 7 ; // Top bit
- if ( SerialControl.speed == SPEED_100K )
- {
- #ifdef INVERT_SERIAL
- byteLo |= 0x02 ; // Parity bit
- #else
- byteLo |= 0xFC ; // Stop bits
- #endif
- // calc parity
- temp = byte ;
- temp >>= 4 ;
- temp = byte ^ temp ;
- temp1 = temp ;
- temp1 >>= 2 ;
- temp = temp ^ temp1 ;
- temp1 = temp ;
- temp1 <<= 1 ;
- temp ^= temp1 ;
- temp &= 0x02 ;
- #ifdef INVERT_SERIAL
- byteLo ^= temp ;
- #else
- byteLo |= temp ;
- #endif
- }
- else
- {
- byteLo |= 0xFE ; // Stop bit
- }
- byte <<= 1 ;
- #ifdef INVERT_SERIAL
- byte |= 1 ; // Start bit
- #endif
- uint8_t next = SerialControl.head + 2;
- if(next>=TXBUFFER_SIZE)
- next=0;
- if ( next != SerialControl.tail )
- {
- SerialControl.data[SerialControl.head] = byte ;
- SerialControl.data[SerialControl.head+1] = byteLo ;
- SerialControl.head = next ;
- }
- if(!IS_TX_PAUSE_on)
- tx_resume();
-}
-
-#endif // TELEMETRY
-
diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp
index 1d94648..9af091e 100644
--- a/remote/src/FrSkyD_cc2500.cpp
+++ b/remote/src/FrSkyD_cc2500.cpp
@@ -17,10 +17,20 @@
#include "Multiprotocol.h"
#include "input.h"
#include "FrSkyD_cc2500.h"
-
+#include "telemetry.h"
+uint8_t v_lipo1;
+uint8_t v_lipo2;
+uint8_t RX_RSSI;
+uint8_t TX_RSSI;
+uint8_t RX_LQI;
+uint8_t TX_LQI;
+uint8_t telemetry_link=0;
+uint8_t telemetry_lost;
+uint8_t telemetry_counter=0;
uint8_t rx_tx_addr[5];
uint8_t hopping_frequency[50];
uint8_t pkt[MAX_PKT];//telemetry receiving packets
+uint8_t pktt[MAX_PKT];//telemetry receiving packets
uint16_t state;
uint16_t counter;
diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp
index 06ca903..dfd7e51 100644
--- a/remote/src/state_fly.cpp
+++ b/remote/src/state_fly.cpp
@@ -2,6 +2,7 @@
#include
#include "Arduino.h"
#include "FrSkyD_cc2500.h"
+#include "telemetry.h"
#include "state.h"
#include "input.h"
#include "eeprom.h"
@@ -47,18 +48,38 @@ void LCD_state_fly::enter(void) {
lcd.setCursor(6,1);
lcd.write(rssiantenna);
- byte rssi_bars_[] = {
- 0b00001,
- 0b00001,
- 0b00001,
- 0b00001,
- 0b00101,
- 0b00101,
- 0b10101,
- 0b10101 };
+
+ byte rssi_bars_[8];
+ rssi_bars_[0] = 0b00001;
+ rssi_bars_[1] = 0b00001;
+ rssi_bars_[2] = 0b00001;
+ rssi_bars_[3] = 0b00001;
+ rssi_bars_[4] = 0b00101;
+ rssi_bars_[5] = 0b00101;
+ rssi_bars_[6] = 0b10101;
+ rssi_bars_[7] = 0b10101;
+
lcd.createChar(rssi_bars, rssi_bars_);
+
lcd.setCursor(7,1);
lcd.write(rssi_bars);
+
+ byte battery_char_data[8];
+ battery_char_data[0] = 0b01110;
+ battery_char_data[1] = 0b11011;
+ battery_char_data[2] = 0b10001;
+ battery_char_data[3] = 0b10001;
+ battery_char_data[4] = 0b10001;
+ battery_char_data[5] = 0b10001;
+ battery_char_data[6] = 0b10001;
+ battery_char_data[7] = 0b11111;
+ lcd.createChar(battery_char, battery_char_data);
+
+ lcd.setCursor(12,1);
+ lcd.write(battery_char);
+
+ lcd.setCursor(12,0);
+ lcd.write(battery_char);
}
void LCD_state_fly::print_time(uint16_t time)
@@ -70,7 +91,7 @@ void LCD_state_fly::print_time(uint16_t time)
* fly mode A PP
* T SSS AA PP A PP
**/
- if(this->last_time==time)
+ if(this->last_time == time)
return;
this->last_time=time;
@@ -79,37 +100,37 @@ void LCD_state_fly::print_time(uint16_t time)
snprintf(line, sizeof(line), "%*u", 3, time);
lcd.print(line);
}
-void LCD_state_fly::print_akku(uint8_t akku_quad, uint8_t akku_remote)
+void LCD_state_fly::print_akku_quad(uint8_t akku_quad)
+{
+ /**
+ * 0123456789012345
+ * fly mode A PP
+ * T SSS AA PP A PP
+ **/
+ char line[17];
+
+ lcd.setCursor(13,1);
+ snprintf(line, sizeof(line), "%*d", 3, akku_quad);
+ lcd.print(line);
+ return;
+}
+
+void LCD_state_fly::print_akku_remote(uint8_t akku_remote)
{
/**
* 0123456789012345
* fly mode A PP
* T SSS AA PP A PP
**/
- byte battery_char_data[8];
char line[17];
- uint8_t akku[] = {akku_remote, akku_quad};
- for (int i = 0; i < 2; ++i) {
- battery_char_data[0] = 0b01110;
- battery_char_data[1] = ( akku[i] > 90) ? 0b11111 : 0b11011;
- battery_char_data[2] = ( akku[i] > 75) ? 0b11111 : 0b10001;
- battery_char_data[3] = ( akku[i] > 60) ? 0b11111 : 0b10001;
- battery_char_data[4] = ( akku[i] > 45) ? 0b11111 : 0b10001;
- battery_char_data[5] = ( akku[i] > 30) ? 0b11111 : 0b10001;
- battery_char_data[6] = ( akku[i] > 15) ? 0b11111 : 0b10001;
- battery_char_data[7] = 0b11111;
- lcd.createChar(battery_char+i, battery_char_data);
-
- lcd.setCursor(12,i);
- lcd.write(battery_char+i);
-
- lcd.setCursor(13,i);
- snprintf(line, sizeof(line), "%*d", 3, akku[i]);
- lcd.print(line);
- }
+
+ lcd.setCursor(13,0);
+ snprintf(line, sizeof(line), "%*d", 3, akku_remote);
+ lcd.print(line);
return;
}
+
void LCD_state_fly::print_rssi(uint8_t rssi_percent)
{
char line[17];
@@ -125,12 +146,12 @@ void LCD_state_fly::update(void)
uint8_t rssi_percent = 100;
uint8_t akku_quad = 1;
uint8_t akku_remote = 2;
- char line[17];
unsigned long time_in_ms = millis() - this->time_enter;
unsigned long time_in_s = time_in_ms/1000; // to sec
this->print_time(time_in_s);
- this->print_akku(akku_quad, akku_remote);
+ this->print_akku_quad(akku_quad);
+ this->print_akku_remote(akku_remote);
this->print_rssi(rssi_percent);
uint32_t end__ = micros();
@@ -148,7 +169,7 @@ void LCD_state_fly::update(void)
next_callback_time = ReadFrSky_2way();
//if (next_callback_time > 9000) {
- if (next_callback_time > 9000) {
+ if (next_callback_time == 9200) {
input.update();
if (input.is_menu_triggered()) {
@@ -156,39 +177,48 @@ void LCD_state_fly::update(void)
input.print();
break;
}
+ }
+ if (next_callback_time == 9200) {
// print on lcd
- call +=1;
- if(call > 5)
+ call +=2;
+ if(call > 50)
call= 0;
switch(call)
{
- case 1:
+ case 10:
rssi_percent += 1;
if(rssi_percent > 100)
rssi_percent = 0;
this->print_rssi(rssi_percent);
break;
- case 2:
+ case 20:
// update time
time_in_ms = millis() - this->time_enter;
time_in_s = time_in_ms/1000; // to sec
this->print_time(time_in_s);
break;
- case 3:
+ case 30:
// update akku
akku_quad += 1;
- akku_remote += 2;
if(akku_quad > 100)
akku_quad = 0;
+ this->print_akku_quad(akku_quad);
+ break;
+
+ case 40:
+ // update akku
+ akku_remote += 2;
if(akku_remote > 100)
akku_remote = 0;
- this->print_akku(akku_quad, akku_remote);
+ this->print_akku_remote(akku_remote);
break;
+ case 50:
+ //print_frskyd_telemetry();
default:
break;
@@ -202,6 +232,8 @@ void LCD_state_fly::update(void)
uint32_t wait = next_callback_time;
wait -= (end__ - start);
delayMicroseconds(wait);
+ if((end__ - start) > 1000 )
+ debug("call %d waited %lu timed %lu vs wait %lu \n", call,next_callback_time, wait, (end__ - start));
}
end__ = micros();
}
diff --git a/remote/src/state_joy_calib.cpp b/remote/src/state_joy_calib.cpp
index bfc959e..a4e4861 100644
--- a/remote/src/state_joy_calib.cpp
+++ b/remote/src/state_joy_calib.cpp
@@ -105,7 +105,7 @@ void LCD_state_joy_calibration::update(void) {
#define STM32_UUID ((uint32_t *)0x1FFFF7E8)
id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2];
#endif
- debugln("Generated new master ID %x", id);
+ debugln("Generated new master ID %lx", id);
eeprom_config.set_master_id(id);
eeprom_config.write();
diff --git a/remote/src/telemetry.cpp b/remote/src/telemetry.cpp
new file mode 100644
index 0000000..d665307
--- /dev/null
+++ b/remote/src/telemetry.cpp
@@ -0,0 +1,227 @@
+/*
+ This project is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Multiprotocol is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Multiprotocol. If not, see .
+ */
+//**************************
+// Telemetry serial code *
+//**************************
+#include "telemetry.h"
+#include "FrSkyD_cc2500.h"
+#include "debug.h"
+uint8_t RetrySequence ;
+
+#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
+ #define MULTI_TIME 500 //in ms
+ #define INPUT_SYNC_TIME 100 //in ms
+ #define INPUT_ADDITIONAL_DELAY 100 // in 10µs, 100 => 1000 µs
+ uint32_t lastMulti = 0;
+#endif // MULTI_TELEMETRY/MULTI_STATUS
+
+#if defined SPORT_TELEMETRY
+ #define SPORT_TIME 12000 //12ms
+ #define FRSKY_SPORT_PACKET_SIZE 8
+ #define FX_BUFFERS 4
+ uint32_t last = 0;
+ uint8_t sport_counter=0;
+ uint8_t RxBt = 0;
+ uint8_t sport = 0;
+ uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
+
+ // Store for out of sequence packet
+ uint8_t FrskyxRxTelemetryValidSequence ;
+ struct t_fx_rx_frame
+ {
+ uint8_t valid ;
+ uint8_t count ;
+ uint8_t payload[6] ;
+ } ;
+
+ // Store for FrskyX telemetry
+ struct t_fx_rx_frame FrskyxRxFrames[4] ;
+ uint8_t NextFxFrameToForward ;
+ #ifdef SPORT_POLLING
+ uint8_t sport_rx_index[28] ;
+ uint8_t ukindex ;
+ uint8_t kindex ;
+ uint8_t TxData[2];
+ uint8_t SportIndexPolling;
+ uint8_t RxData[16] ;
+ volatile uint8_t RxIndex=0 ;
+ uint8_t sport_bytes=0;
+ uint8_t skipped_id;
+ uint8_t rx_counter=0;
+ #endif
+#endif // SPORT_TELEMETRY
+
+#if defined HUB_TELEMETRY
+ #define USER_MAX_BYTES 6
+ uint8_t prev_index;
+#endif // HUB_TELEMETRY
+
+#define START_STOP 0x7e
+#define BYTESTUFF 0x7d
+#define STUFF_MASK 0x20
+#define MAX_PKTX 10
+uint8_t pktx[MAX_PKTX];
+uint8_t indx;
+uint8_t frame[18];
+
+#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
+static void multi_send_header(uint8_t type, uint8_t len)
+{
+ Serial_write('M');
+ #ifdef MULTI_TELEMETRY
+ Serial_write('P');
+ Serial_write(type);
+ #else
+ (void)type;
+ #endif
+ Serial_write(len);
+}
+
+#endif
+
+#ifdef MULTI_TELEMETRY
+static void multi_send_frskyhub()
+{
+ multi_send_header(MULTI_TELEMETRY_HUB, 9);
+ for (uint8_t i = 0; i < 9; i++)
+ Serial_write(frame[i]);
+}
+#endif
+
+
+void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
+{
+ uint8_t clen = pkt[0] + 3 ;
+ if (len != clen) // wrong length
+ return;
+ if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] ) {
+ telemetry_link |= 1; // Telemetry data is available
+ TX_RSSI = pkt[len-2];
+
+ if(TX_RSSI >=128)
+ TX_RSSI -= 128;
+ else
+ TX_RSSI += 128;
+
+ TX_LQI = pkt[len-1]&0x7F;
+ for (uint8_t i=3;i0 && pktt[6]<=10) {
+ if ( ( pktt[7] & 0x1F ) == (telemetry_counter & 0x1F) ) {
+ uint8_t topBit = 0 ;
+ if ( telemetry_counter & 0x80 )
+ if ( ( telemetry_counter & 0x1F ) != RetrySequence )
+ topBit = 0x80 ;
+ telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
+ } else {
+ // incorrect sequence
+ RetrySequence = pktt[7] & 0x1F ;
+ telemetry_counter |= 0x80 ;
+ pktt[6]=0 ; // Discard current packet and wait for retransmit
+ }
+ } else
+ pktt[6]=0; // Discard packet
+ }
+}
+
+void print_frskyd_telemetry() {
+ debug("telemetry_link %d\n", telemetry_link);
+ debug("telemetry_counter %d\n", telemetry_counter);
+ debug("v_lipo1 %d\n", v_lipo1 );
+ debug("v_lipo2 %d\n", v_lipo2 );
+ debug("RX_RSSI %d\n", RX_RSSI );
+ debug("TX_RSSI %d\n", TX_RSSI );
+ debug("RX_LQI %d\n", RX_LQI );
+ debug("TX_LQI %d\n", TX_LQI );
+}
+void init_frskyd_link_telemetry()
+{
+ telemetry_link=0;
+ telemetry_counter=0;
+ v_lipo1=0;
+ v_lipo2=0;
+ RX_RSSI=0;
+ TX_RSSI=0;
+ RX_LQI=0;
+ TX_LQI=0;
+}
+
+#if defined SPORT_TELEMETRY
+/* SPORT details serial
+ 100K 8E2 normal-multiprotocol
+ -every 12ms-or multiple of 12; %36
+ 1 2 3 4 5 6 7 8 9 CRC DESCR
+ 7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
+ 7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
+ 7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+ 7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
+
+
+ Telemetry frames(RF) SPORT info
+ 15 bytes payload
+ SPORT frame valid 6+3 bytes
+ [00] PKLEN 0E 0E 0E 0E
+ [01] TXID1 DD DD DD DD
+ [02] TXID2 6D 6D 6D 6D
+ [03] CONST 02 02 02 02
+ [04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
+ [05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
+ [06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
+ [07] STRM1 00 00 7E 00
+ [08] STRM2 00 00 1A 00
+ [09] STRM3 00 00 10 00
+ [10] STRM4 03 03 03 03
+ [11] STRM5 F1 F1 F1 F1
+ [12] STRM6 D1 D1 D0 D0
+ [13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
+ [14] CHKSUM2 --|
+ +2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
+
+0x06 0x06 0x06 0x06 0x06
+
+0x7E 0x00 0x03 0x7E 0x00
+0x1A 0x00 0xF1 0x1A 0x00
+0x10 0x00 0xD7 0x10 0x00
+0x03 0x7E 0x00 0x03 0x7E
+0xF1 0x1A 0x00 0xF1 0x1A
+0xD7 0x10 0x00 0xD7 0x10
+
+0xE1 0x1C 0xD0 0xEE 0x33
+0x34 0x0A 0xC3 0x56 0xF3
+
+ */
+#if defined SPORT_POLLING || defined MULTI_TELEMETRY
+const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
+ 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
+ 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
+ 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
+ 0x98, 0x39, 0xBA, 0x1B } ;
+#endif
+
+
+
+#endif
+
+/**************************/
+/**************************/
+/** Serial TX routines **/
+/**************************/
+/**************************/