From 84b458f1ef108a5a0f3830cf4d977be42beb9029 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 13:43:41 +0200 Subject: [PATCH] add telemety draft and reduce lcd update --- remote/include/FrSkyD_cc2500.h | 12 +- remote/include/Multiprotocol.h | 1 - remote/include/state.h | 7 +- remote/include/telemetry.h | 359 +-------------------------------- remote/src/FrSkyD_cc2500.cpp | 12 +- remote/src/state_fly.cpp | 114 +++++++---- remote/src/state_joy_calib.cpp | 2 +- remote/src/telemetry.cpp | 227 +++++++++++++++++++++ 8 files changed, 334 insertions(+), 400 deletions(-) create mode 100644 remote/src/telemetry.cpp 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 **/ +/**************************/ +/**************************/