Philipp Schönberger
6 years ago
committed by
GitHub
GitHub
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
31 changed files with 1592 additions and 1398 deletions
-
383d/quad_charge_plug.scad
-
143d/stick.scad
-
36remote/include/FrSkyD_cc2500.h
-
16remote/include/Multiprotocol.h
-
10remote/include/Validate.h
-
63remote/include/common.h
-
80remote/include/config.h
-
5remote/include/crc.h
-
2remote/include/debug.h
-
92remote/include/eeprom.h
-
58remote/include/input.h
-
31remote/include/pins.h
-
27remote/include/state.h
-
18remote/include/telemetry.h
-
36remote/include/tx_def.h
-
198remote/src/FrSkyD_cc2500.cpp
-
433remote/src/Multiprotocol.cpp
-
136remote/src/common.ds
-
1remote/src/config.cpp
-
28remote/src/crc.cpp
-
107remote/src/eeprom.cpp
-
109remote/src/input.cpp
-
337remote/src/state.cpp
-
78remote/src/state_bind.cpp
-
256remote/src/state_fly.cpp
-
58remote/src/state_init.cpp
-
134remote/src/state_joy_calib.cpp
-
64remote/src/state_joy_usb.cpp
-
86remote/src/state_menu.cpp
-
227remote/src/telemetry.cpp
@ -1,63 +0,0 @@ |
|||||
/* |
|
||||
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 <http://www.gnu.org/licenses/>. |
|
||||
*/ |
|
||||
#ifndef _COMMON_H_ |
|
||||
#define _COMMON_H_ |
|
||||
#include <stdint.h> |
|
||||
#include "config.h" |
|
||||
#include "tx_def.h" |
|
||||
|
|
||||
void InitChannel(void); |
|
||||
void reverse_channel(uint8_t num); |
|
||||
uint16_t convert_channel_ppm(uint8_t num); |
|
||||
uint16_t convert_channel_10b(uint8_t num); |
|
||||
uint8_t convert_channel_8b(uint8_t num); |
|
||||
int16_t convert_channel_16b_limit(uint8_t num, int16_t min, int16_t max); |
|
||||
int16_t convert_channel_16b_nolimit(uint8_t num, int16_t min, int16_t max); |
|
||||
uint8_t convert_channel_s8b(uint8_t num); |
|
||||
uint16_t limit_channel_100(uint8_t num); |
|
||||
void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high); |
|
||||
void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high); |
|
||||
uint16_t convert_channel_frsky(uint8_t num); |
|
||||
|
|
||||
/******************************/ |
|
||||
/** FrSky D and X routines **/ |
|
||||
/******************************/ |
|
||||
enum { |
|
||||
FRSKY_BIND = 0, |
|
||||
FRSKY_BIND_DONE = 1000, |
|
||||
FRSKY_DATA1, |
|
||||
FRSKY_DATA2, |
|
||||
FRSKY_DATA3, |
|
||||
FRSKY_DATA4, |
|
||||
FRSKY_DATA5 |
|
||||
}; |
|
||||
|
|
||||
void Frsky_init_hop(void); |
|
||||
/******************************/ |
|
||||
/** FrSky V, D and X routines **/ |
|
||||
/******************************/ |
|
||||
#if defined(FRSKYV_CC2500_INO) || defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) |
|
||||
|
|
||||
extern uint8_t FRSKY_common_startreg_cc2500_conf[]; |
|
||||
|
|
||||
extern uint8_t FRSKYD_cc2500_conf[]; |
|
||||
|
|
||||
|
|
||||
extern uint8_t FRSKY_common_end_cc2500_conf[][2]; |
|
||||
void FRSKY_init_cc2500(const uint8_t *ptr); |
|
||||
|
|
||||
#endif |
|
||||
|
|
||||
#endif |
|
||||
@ -0,0 +1,5 @@ |
|||||
|
#ifndef __CRC32_H_ |
||||
|
#define __CRC32_H_ |
||||
|
#include <Arduino.h> |
||||
|
uint32_t tiny_crc32(const void *data, unsigned int length); |
||||
|
#endif |
||||
@ -1,81 +1,37 @@ |
|||||
#ifndef _EEPROM_H_ |
#ifndef _EEPROM_H_ |
||||
#define _EEPROM_H_ |
#define _EEPROM_H_ |
||||
#include <EEPROM.h> |
|
||||
|
|
||||
#include <Arduino.h> |
|
||||
#include <stdint.h> |
|
||||
#include "tx_def.h" |
|
||||
|
#include "input.h" |
||||
|
extern class Eeprom_config eeprom_config; |
||||
|
|
||||
#define CURRENT_VERSION 0x01 |
|
||||
|
class Eeprom_config { |
||||
|
public: |
||||
|
|
||||
|
Eeprom_config(void); |
||||
|
~Eeprom_config(void); |
||||
|
|
||||
|
int validate(void); |
||||
|
int read(void); |
||||
|
int write(void); |
||||
|
|
||||
|
int get_ch_config(struct Input::ch_config* config); |
||||
|
int set_ch_config(struct Input::ch_config* config); |
||||
|
|
||||
|
int get_master_id(uint32_t* master_id); |
||||
|
int set_master_id(uint32_t master_id); |
||||
|
|
||||
|
private: |
||||
|
#define CURRENT_VERSION 0x02 |
||||
struct eeprom_data_v1 { |
struct eeprom_data_v1 { |
||||
uint8_t version; |
uint8_t version; |
||||
uint32_t crc; |
|
||||
struct { |
|
||||
|
uint32_t data_crc; |
||||
struct { |
struct { |
||||
uint16_t max; |
|
||||
uint16_t min; |
|
||||
uint8_t inverted; |
|
||||
} throttle, yaw, roll, pitch, aux[5]; |
|
||||
uint32_t master_id; |
uint32_t master_id; |
||||
|
struct Input::ch_config ch[Input::CH_COUNT]; |
||||
} data; |
} data; |
||||
|
|
||||
|
} current_config; |
||||
|
bool sucessfull_read; |
||||
}; |
}; |
||||
|
|
||||
|
|
||||
const static uint32_t crc_table[16] = { |
|
||||
0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac, |
|
||||
0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c, |
|
||||
0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c, |
|
||||
0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c |
|
||||
}; |
|
||||
|
|
||||
uint32_t crc_update (uint32_t crc, uint8_t data) |
|
||||
{ |
|
||||
uint8_t tbl_idx; |
|
||||
|
|
||||
tbl_idx = crc ^ (data >> (0 * 4)); |
|
||||
crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); |
|
||||
|
|
||||
tbl_idx = crc ^ (data >> (1 * 4)); |
|
||||
crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); |
|
||||
|
|
||||
return crc; |
|
||||
} |
|
||||
|
|
||||
|
|
||||
int read_eeprom(struct eeprom_data_v1 *eeprom_data) { |
|
||||
uint8_t *data = NULL; |
|
||||
uint8_t start_address = 0x10; |
|
||||
|
|
||||
EEPROM.PageBase0 = 0x801F000; |
|
||||
EEPROM.PageBase1 = 0x801F800; |
|
||||
EEPROM.PageSize = 0x400; |
|
||||
EEPROM.init(); |
|
||||
|
|
||||
data = (uint8_t*) eeprom_data; |
|
||||
|
|
||||
for (uint8_t i = 0; i < sizeof(eeprom_data) ; i = 0) { |
|
||||
EEPROM.read(0x10 + i , data); |
|
||||
data+=1; |
|
||||
} |
|
||||
|
|
||||
// validate version |
|
||||
if (eeprom_data->version != CURRENT_VERSION) { |
|
||||
return -1; |
|
||||
} |
|
||||
|
|
||||
|
|
||||
data = (uint8_t*) &(eeprom_data->data); |
|
||||
uint32_t crc = ~0L; |
|
||||
for (uint8_t i = 0; i < sizeof(eeprom_data->data) ; i = 0) { |
|
||||
crc = crc_update(crc, *data); |
|
||||
data+=1; |
|
||||
} |
|
||||
|
|
||||
// validate crc |
|
||||
if (eeprom_data->crc != crc) { |
|
||||
return -1; |
|
||||
} |
|
||||
|
|
||||
} |
|
||||
#endif |
#endif |
||||
@ -0,0 +1,18 @@ |
|||||
|
#ifndef __TELE_METRY_H_ |
||||
|
#define __TELE_METRY_H_ |
||||
|
#include <Arduino.h> |
||||
|
|
||||
|
#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) |
||||
|
static void multi_send_header(uint8_t type, uint8_t len); |
||||
|
#endif |
||||
|
|
||||
|
#ifdef MULTI_TELEMETRY |
||||
|
static void multi_send_frskyhub(); |
||||
|
#endif |
||||
|
|
||||
|
|
||||
|
void frsky_check_telemetry(uint8_t *pkt,uint8_t len); |
||||
|
|
||||
|
void print_frskyd_telemetry(); |
||||
|
void init_frskyd_link_telemetry(); |
||||
|
#endif |
||||
@ -0,0 +1,28 @@ |
|||||
|
#include "crc.h"
|
||||
|
|
||||
|
static const unsigned int tiny_crc32tab[16] = { |
||||
|
0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac, |
||||
|
0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c, |
||||
|
0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c, |
||||
|
0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c |
||||
|
}; |
||||
|
|
||||
|
|
||||
|
uint32_t __tiny_crc32(const void *data, unsigned int length, uint32_t crc) |
||||
|
{ |
||||
|
const unsigned char *buf = (const unsigned char *)data; |
||||
|
unsigned int i; |
||||
|
|
||||
|
for (i = 0; i < length; ++i) { |
||||
|
crc ^= buf[i]; |
||||
|
crc = tiny_crc32tab[crc & 0x0f] ^ (crc >> 4); |
||||
|
crc = tiny_crc32tab[crc & 0x0f] ^ (crc >> 4); |
||||
|
} |
||||
|
|
||||
|
return crc; |
||||
|
} |
||||
|
uint32_t tiny_crc32(const void *data, unsigned int length) |
||||
|
{ |
||||
|
// return value suitable for passing in next time, for final value invert it
|
||||
|
return __tiny_crc32(data, length, 42) ^ 0xffffff; |
||||
|
} |
||||
@ -0,0 +1,107 @@ |
|||||
|
#include <EEPROM.h>
|
||||
|
|
||||
|
#include <Arduino.h>
|
||||
|
#include <stdint.h>
|
||||
|
#include <debug.h>
|
||||
|
|
||||
|
#include "tx_def.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "crc.h"
|
||||
|
|
||||
|
class Eeprom_config eeprom_config; |
||||
|
|
||||
|
Eeprom_config::Eeprom_config() { |
||||
|
EEPROM.PageBase0 = 0x801F000; |
||||
|
EEPROM.PageBase1 = 0x801F800; |
||||
|
EEPROM.PageSize = 0x400; |
||||
|
EEPROM.init(); |
||||
|
memset(&this->current_config, 0, sizeof(this->current_config)); |
||||
|
this->sucessfull_read = false; |
||||
|
} |
||||
|
|
||||
|
Eeprom_config::~Eeprom_config() { |
||||
|
/* nope */ |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::validate() { |
||||
|
uint32_t crc_calc = 0; |
||||
|
|
||||
|
if (this->sucessfull_read == false) { |
||||
|
debugln("no read\n"); |
||||
|
return -1; |
||||
|
} |
||||
|
|
||||
|
if (this->current_config.version != CURRENT_VERSION) { |
||||
|
debugln("wrong version %lu vs %lu \n", this->current_config.version, CURRENT_VERSION); |
||||
|
return -1; |
||||
|
} |
||||
|
|
||||
|
crc_calc = tiny_crc32(&(this->current_config.data), sizeof(this->current_config.data)); |
||||
|
|
||||
|
if (crc_calc != this->current_config.data_crc) { |
||||
|
debugln("wrong crc %lu vs %lu \n", crc_calc, this->current_config.data_crc); |
||||
|
return -1; |
||||
|
} |
||||
|
debugln("valid config\n"); |
||||
|
return 1; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::read(void) { |
||||
|
uint8_t *data = NULL; |
||||
|
data = (uint8_t *) &this->current_config; |
||||
|
|
||||
|
for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { |
||||
|
data[i] = EEPROM.read(0x10 + i); |
||||
|
} |
||||
|
|
||||
|
this->sucessfull_read = true; |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::write(void) { |
||||
|
uint8_t *data = NULL; |
||||
|
data = (uint8_t *) &this->current_config; |
||||
|
|
||||
|
this->current_config.version = CURRENT_VERSION; |
||||
|
this->current_config.data_crc = tiny_crc32(&(this->current_config.data), sizeof(this->current_config.data)); |
||||
|
|
||||
|
for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { |
||||
|
EEPROM.write(0x10 + i , data[i]); |
||||
|
} |
||||
|
|
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::get_ch_config(struct Input::ch_config* config) { |
||||
|
if (this->sucessfull_read == false && config != NULL) { |
||||
|
return -1; |
||||
|
} |
||||
|
memcpy(config, &this->current_config.data.ch, sizeof(struct Input::ch_config) * Input::CH_COUNT); |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::set_ch_config(struct Input::ch_config* config) { |
||||
|
if (this->sucessfull_read == false && config != NULL) { |
||||
|
return -1; |
||||
|
} |
||||
|
memcpy(this->current_config.data.ch, config, sizeof(struct Input::ch_config) * Input::CH_COUNT); |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::get_master_id(uint32_t* master_id) |
||||
|
{ |
||||
|
if (this->sucessfull_read == false && master_id != NULL) { |
||||
|
return -1; |
||||
|
} |
||||
|
*master_id = this->current_config.data.master_id; |
||||
|
return 0; |
||||
|
} |
||||
|
|
||||
|
int Eeprom_config::set_master_id(uint32_t master_id) |
||||
|
{ |
||||
|
if (this->sucessfull_read == false) { |
||||
|
return -1; |
||||
|
} |
||||
|
this->current_config.data.master_id = master_id; |
||||
|
return 0; |
||||
|
} |
||||
@ -0,0 +1,78 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "FrSkyD_cc2500.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "debug.h"
|
||||
|
#include "tx_def.h"
|
||||
|
#include "config.h"
|
||||
|
|
||||
|
|
||||
|
LCD_state_bind::LCD_state_bind(void) { |
||||
|
this->bind_time = DEFAULT_BIND_TIME; |
||||
|
} |
||||
|
void LCD_state_bind::enter(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("bind mode "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(" "); |
||||
|
this->time_enter = millis(); |
||||
|
} |
||||
|
|
||||
|
void LCD_state_bind::update(void) |
||||
|
{ |
||||
|
char line[17]; |
||||
|
unsigned long time_in_ms = millis() - this->time_enter; |
||||
|
unsigned long time_in_s = time_in_ms/1000; // to sec
|
||||
|
unsigned long remain_s = this->bind_time - time_in_s; |
||||
|
|
||||
|
snprintf(line,sizeof(line),"remaining sec %02lu",remain_s); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(line); |
||||
|
|
||||
|
uint32_t end__ = micros(); |
||||
|
uint32_t start = micros(); |
||||
|
uint32_t next_callback_time; |
||||
|
next_callback_time = initFrSky_2way(); |
||||
|
|
||||
|
// init for bind
|
||||
|
frsky2way_init(1); |
||||
|
while(1) { |
||||
|
start = end__; |
||||
|
next_callback_time = ReadFrSky_2way_bind(); |
||||
|
|
||||
|
|
||||
|
// update time
|
||||
|
time_in_ms = millis() - this->time_enter; |
||||
|
time_in_s = time_in_ms/1000; // to sec
|
||||
|
remain_s = this->bind_time - time_in_s; |
||||
|
|
||||
|
// print on lcd
|
||||
|
snprintf(line,sizeof(line),"%02lu",remain_s); |
||||
|
lcd.setCursor(14,1); |
||||
|
lcd.print(line); |
||||
|
|
||||
|
end__ = micros(); |
||||
|
|
||||
|
if (end__ - start < next_callback_time) { |
||||
|
uint32_t wait = next_callback_time; |
||||
|
wait -= ((end__-start)); |
||||
|
delayMicroseconds(wait); |
||||
|
end__ += wait; |
||||
|
} |
||||
|
end__ = micros(); |
||||
|
|
||||
|
if (remain_s == 0) |
||||
|
break; |
||||
|
} |
||||
|
|
||||
|
// cange to menu when done
|
||||
|
new_state = s_menu; |
||||
|
} |
||||
|
|
||||
|
void LCD_state_bind::leave(void) |
||||
|
{ |
||||
|
lcd.clear(); |
||||
|
} |
||||
@ -0,0 +1,256 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "FrSkyD_cc2500.h"
|
||||
|
#include "telemetry.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "debug.h"
|
||||
|
#include "tx_def.h"
|
||||
|
#include "pins.h"
|
||||
|
|
||||
|
int16_t map16b(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); |
||||
|
|
||||
|
LCD_state_fly::LCD_state_fly(void) { |
||||
|
} |
||||
|
|
||||
|
void LCD_state_fly::enter(void) { |
||||
|
|
||||
|
this->last_time = 0; |
||||
|
|
||||
|
lcd.clear(); |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("fly mode "); |
||||
|
this->time_enter = millis(); |
||||
|
|
||||
|
byte clock_char_data[] = { |
||||
|
0b01110, |
||||
|
0b10101, |
||||
|
0b10101, |
||||
|
0b10111, |
||||
|
0b10001, |
||||
|
0b10001, |
||||
|
0b01110, |
||||
|
0b00000 |
||||
|
}; |
||||
|
lcd.createChar(clock_char, clock_char_data); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.write(clock_char); |
||||
|
|
||||
|
byte rssi_antenna_[] = { |
||||
|
0b10101, |
||||
|
0b10001, |
||||
|
0b01110, |
||||
|
0b00100, |
||||
|
0b00100, |
||||
|
0b00100, |
||||
|
0b00101, |
||||
|
0b00101, }; |
||||
|
lcd.createChar(rssiantenna, rssi_antenna_); |
||||
|
|
||||
|
|
||||
|
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_); |
||||
|
|
||||
|
|
||||
|
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,0); |
||||
|
lcd.write(battery_char); |
||||
|
#if 0
|
||||
|
lcd.setCursor(12,1); |
||||
|
lcd.write(battery_char); |
||||
|
lcd.setCursor(6,1); |
||||
|
lcd.write(rssiantenna); |
||||
|
lcd.setCursor(7,1); |
||||
|
lcd.write(rssi_bars); |
||||
|
#endif
|
||||
|
} |
||||
|
|
||||
|
void LCD_state_fly::print_time(uint16_t time) |
||||
|
{ |
||||
|
char line[17]; |
||||
|
|
||||
|
/**
|
||||
|
* 0123456789012345 |
||||
|
* fly mode A PP |
||||
|
* T SSS AA PP A PP |
||||
|
**/ |
||||
|
if(this->last_time == time) |
||||
|
return; |
||||
|
|
||||
|
this->last_time=time; |
||||
|
|
||||
|
lcd.setCursor(1,1); |
||||
|
snprintf(line, sizeof(line), "%*u", 3, time); |
||||
|
lcd.print(line); |
||||
|
} |
||||
|
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 |
||||
|
**/ |
||||
|
|
||||
|
char line[17]; |
||||
|
|
||||
|
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]; |
||||
|
|
||||
|
lcd.setCursor(8,1); |
||||
|
snprintf(line, sizeof(line), "%*d", 3, rssi_percent); |
||||
|
lcd.print(line); |
||||
|
} |
||||
|
|
||||
|
void LCD_state_fly::update(void) |
||||
|
{ |
||||
|
uint8_t call=0; |
||||
|
uint8_t rssi_percent = 100; |
||||
|
uint8_t akku_quad = 1; |
||||
|
uint8_t akku_remote = 2; |
||||
|
unsigned long time_in_ms = millis() - this->time_enter; |
||||
|
unsigned long time_in_s = time_in_ms/1000; // to sec
|
||||
|
|
||||
|
uint32_t end__ = micros(); |
||||
|
uint32_t start = micros(); |
||||
|
uint32_t next_callback_time; |
||||
|
next_callback_time = initFrSky_2way(); |
||||
|
|
||||
|
input.update(); |
||||
|
// init for bind
|
||||
|
frsky2way_init(1); |
||||
|
|
||||
|
input.print(); |
||||
|
while(1) { |
||||
|
start = end__; |
||||
|
next_callback_time = ReadFrSky_2way(); |
||||
|
|
||||
|
//if (next_callback_time > 9000) {
|
||||
|
if (next_callback_time == 9200) { |
||||
|
input.update(); |
||||
|
|
||||
|
if (input.is_menu_triggered()) { |
||||
|
debug("%lu menu button trigger\n", millis()); |
||||
|
input.print(); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
|
||||
|
if (next_callback_time == 9200) { |
||||
|
// print on lcd
|
||||
|
call +=2; |
||||
|
if(call > 50) |
||||
|
call= 0; |
||||
|
|
||||
|
switch(call) |
||||
|
{ |
||||
|
|
||||
|
|
||||
|
case 10: |
||||
|
// 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; |
||||
|
|
||||
|
#if 0
|
||||
|
case 20: |
||||
|
rssi_percent += 1; |
||||
|
if(rssi_percent > 100) |
||||
|
rssi_percent = 0; |
||||
|
this->print_rssi(rssi_percent); |
||||
|
break; |
||||
|
|
||||
|
case 30: |
||||
|
// update akku
|
||||
|
akku_quad += 1; |
||||
|
if(akku_quad > 100) |
||||
|
akku_quad = 0; |
||||
|
this->print_akku_quad(akku_quad); |
||||
|
break; |
||||
|
|
||||
|
#endif
|
||||
|
case 40: |
||||
|
// update akku
|
||||
|
akku_remote = analogRead(Battery_pin); |
||||
|
if (akku_remote > 3830) |
||||
|
akku_remote = 3830; |
||||
|
if (akku_remote < 3450) |
||||
|
akku_remote = 3450; |
||||
|
akku_remote = map16b(akku_remote, 3450, 3830, 0, 100); |
||||
|
if (akku_remote > 100) |
||||
|
akku_remote = 100; |
||||
|
this->print_akku_remote(akku_remote); |
||||
|
break; |
||||
|
|
||||
|
default: |
||||
|
break; |
||||
|
} |
||||
|
|
||||
|
} |
||||
|
|
||||
|
|
||||
|
end__ = micros(); |
||||
|
if (end__ - start < next_callback_time) { |
||||
|
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)); */ |
||||
|
/* print_frskyd_telemetry(); */ |
||||
|
/* } */ |
||||
|
} |
||||
|
end__ = micros(); |
||||
|
} |
||||
|
|
||||
|
// cange to menu when done
|
||||
|
new_state = s_menu; |
||||
|
|
||||
|
} |
||||
|
void LCD_state_fly::leave(void) |
||||
|
{ |
||||
|
lcd.clear(); |
||||
|
} |
||||
@ -0,0 +1,58 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "FrSkyD_cc2500.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "debug.h"
|
||||
|
#include "tx_def.h"
|
||||
|
|
||||
|
LCD_state_init::LCD_state_init(void) { |
||||
|
} |
||||
|
void LCD_state_init::enter(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print(" wellcome "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(" phschoen "); |
||||
|
this->time_enter = millis(); |
||||
|
} |
||||
|
void LCD_state_init::update(void) |
||||
|
{ |
||||
|
uint32_t diff; |
||||
|
delay(100); |
||||
|
diff = millis() - this->time_enter; |
||||
|
if (diff > 3 * 1000) { |
||||
|
new_state = s_joy; |
||||
|
debugln("read start"); |
||||
|
delay(1000); |
||||
|
if ( 0 > eeprom_config.read()) { |
||||
|
debugln("failed to read"); |
||||
|
return; |
||||
|
} |
||||
|
debugln("read fin"); |
||||
|
|
||||
|
debugln("val start"); |
||||
|
if ( 0 > eeprom_config.validate()) { |
||||
|
debugln("failed to validate"); |
||||
|
return; |
||||
|
} |
||||
|
debugln("val fin"); |
||||
|
|
||||
|
new_state = s_menu; |
||||
|
|
||||
|
struct Input::ch_config ch_config[Input::CH_COUNT]; |
||||
|
eeprom_config.get_ch_config(ch_config); |
||||
|
input.set_calibration(ch_config); |
||||
|
|
||||
|
uint32_t master_id = 0; |
||||
|
eeprom_config.get_master_id(&master_id); |
||||
|
set_rx_tx_addr(master_id); |
||||
|
|
||||
|
|
||||
|
} |
||||
|
} |
||||
|
void LCD_state_init::leave(void) |
||||
|
{ |
||||
|
lcd.clear(); |
||||
|
} |
||||
@ -0,0 +1,134 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "FrSkyD_cc2500.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "debug.h"
|
||||
|
#include "tx_def.h"
|
||||
|
|
||||
|
LCD_state_joy_calibration::LCD_state_joy_calibration(void) { |
||||
|
|
||||
|
} |
||||
|
void LCD_state_joy_calibration::enter(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("calib. start "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print("move all sticks "); |
||||
|
delay(500); |
||||
|
} |
||||
|
void show_dots(int row, int number) { |
||||
|
lcd.setCursor(0,row); |
||||
|
for(int c = 0; c < 16;++c) { |
||||
|
if (c < number) |
||||
|
lcd.print("."); |
||||
|
else |
||||
|
lcd.print(" "); |
||||
|
} |
||||
|
} |
||||
|
void LCD_state_joy_calibration::update(void) { |
||||
|
int8_t turns = 50; |
||||
|
int8_t i; |
||||
|
|
||||
|
// init min/max
|
||||
|
input.calibration_reset(); |
||||
|
|
||||
|
// min max calibration
|
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("min max Calib. "); |
||||
|
show_dots(1,16); |
||||
|
i = turns; |
||||
|
while(i > 0) { |
||||
|
input.update(); |
||||
|
if (true == input.calibration_update()) { |
||||
|
i = turns; |
||||
|
}else { |
||||
|
i -= 1; |
||||
|
show_dots(1,(i *16)/turns); |
||||
|
} |
||||
|
delay(100); |
||||
|
} |
||||
|
|
||||
|
// center
|
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("center sticks "); |
||||
|
i = turns; |
||||
|
while(i > 0) { |
||||
|
input.update(); |
||||
|
if (false == input.is_centered()) { |
||||
|
i = turns; |
||||
|
}else { |
||||
|
i -= 1; |
||||
|
show_dots(1,(i *16)/turns); |
||||
|
} |
||||
|
delay(100); |
||||
|
} |
||||
|
|
||||
|
for (uint8_t _ch = 0; _ch < 4 ; ++_ch) { |
||||
|
enum Input::input_channels ch = (enum Input::input_channels) _ch; |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("move to max: "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(" "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(ch_name[ch]); |
||||
|
|
||||
|
i = turns; |
||||
|
while(i>0) { |
||||
|
delay(50); |
||||
|
input.update(); |
||||
|
|
||||
|
input.print_ch(ch); |
||||
|
if (input.is_high(ch)) { |
||||
|
i--; |
||||
|
continue; |
||||
|
} |
||||
|
|
||||
|
if (input.is_low(ch)) { |
||||
|
input.invert_ch(ch); |
||||
|
debug("invert"); |
||||
|
i = turns; |
||||
|
continue; |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
struct Input::ch_config ch_config[Input::CH_COUNT]; |
||||
|
input.get_calibration(ch_config); |
||||
|
eeprom_config.set_ch_config(ch_config); |
||||
|
uint32_t id=0; |
||||
|
#ifdef FORCE_GLOBAL_ID
|
||||
|
id = FORCE_GLOBAL_ID; |
||||
|
#else
|
||||
|
// Generate a random ID from UUID
|
||||
|
#define STM32_UUID ((uint32_t *)0x1FFFF7E8)
|
||||
|
id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; |
||||
|
#endif
|
||||
|
debugln("Generated new master ID %lx", id); |
||||
|
eeprom_config.set_master_id(id); |
||||
|
eeprom_config.write(); |
||||
|
|
||||
|
eeprom_config.read(); |
||||
|
if (eeprom_config.validate()) { |
||||
|
debugln("ok calib\n"); |
||||
|
}else { |
||||
|
debugln("failed calib\n"); |
||||
|
} |
||||
|
|
||||
|
|
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("center again "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print("all sticks "); |
||||
|
while (false == input.is_centered()) { |
||||
|
input.update(); |
||||
|
} |
||||
|
new_state = s_menu; |
||||
|
} |
||||
|
void LCD_state_joy_calibration::leave(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("finished "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print("calibration"); |
||||
|
} |
||||
@ -0,0 +1,64 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "debug.h"
|
||||
|
|
||||
|
int16_t map16b( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); |
||||
|
#include <USBComposite.h>
|
||||
|
extern HIDJoystick Joystick; |
||||
|
LCD_state_joy_usb::LCD_state_joy_usb(void) { |
||||
|
} |
||||
|
void LCD_state_joy_usb::enter(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("joystick mode "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(" "); |
||||
|
delay(500); |
||||
|
} |
||||
|
void LCD_state_joy_usb::update(void) { |
||||
|
input.update(); |
||||
|
|
||||
|
uint16_t *ch_data= input.get_channel_data(); |
||||
|
|
||||
|
uint16_t x = map16b( ch_data[Input::CH_THROTTLE], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); |
||||
|
uint16_t y = map16b( ch_data[Input::CH_YAW], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); |
||||
|
|
||||
|
uint16_t xr = map16b( ch_data[Input::CH_ROLL], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); |
||||
|
uint16_t yr = map16b( ch_data[Input::CH_PITCH], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); |
||||
|
|
||||
|
bool bt0 = ch_data[Input::CH_AUX1] == CHANNEL_MIN_100; |
||||
|
bool bt1 = ch_data[Input::CH_AUX2] == CHANNEL_MIN_100; |
||||
|
bool bt2 = ch_data[Input::CH_AUX3] == CHANNEL_MIN_100; |
||||
|
bool bt3 = ch_data[Input::CH_AUX4] == CHANNEL_MIN_100; |
||||
|
bool bt4 = ch_data[Input::CH_AUX5] == CHANNEL_MIN_100; |
||||
|
delay(50); |
||||
|
|
||||
|
Joystick.X(x); |
||||
|
Joystick.Y(y); |
||||
|
Joystick.Xrotate(xr); |
||||
|
Joystick.Yrotate(yr); |
||||
|
Joystick.button(0, bt0); |
||||
|
Joystick.button(1, bt1); |
||||
|
Joystick.button(2, bt2); |
||||
|
Joystick.button(3, bt3); |
||||
|
Joystick.button(4, bt4); |
||||
|
|
||||
|
char line[17]; |
||||
|
snprintf(line,sizeof(line),"%lu %lu", bt0 , input.ch_raw[Input::CH_AUX1]); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(line); |
||||
|
|
||||
|
|
||||
|
if (input.is_menu_triggered()) { |
||||
|
debug("%lu menu button trigger\n", millis); |
||||
|
new_state = s_menu; |
||||
|
} |
||||
|
} |
||||
|
void LCD_state_joy_usb::leave(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("finished "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print("usb mode "); |
||||
|
} |
||||
@ -0,0 +1,86 @@ |
|||||
|
#include <LiquidCrystal_I2C.h>
|
||||
|
#include <stdio.h>
|
||||
|
#include "Arduino.h"
|
||||
|
#include "FrSkyD_cc2500.h"
|
||||
|
#include "state.h"
|
||||
|
#include "input.h"
|
||||
|
#include "eeprom.h"
|
||||
|
#include "debug.h"
|
||||
|
#include "tx_def.h"
|
||||
|
|
||||
|
LCD_state_menu::LCD_state_menu(void) { |
||||
|
} |
||||
|
void LCD_state_menu::enter(void) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("menu mode "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(" "); |
||||
|
this->curr_selected = 0; |
||||
|
} |
||||
|
|
||||
|
void LCD_state_menu::update(void) |
||||
|
{ |
||||
|
struct { |
||||
|
char name[15]; |
||||
|
State * state; |
||||
|
} menus[] = { |
||||
|
{ "Flight ", s_fly }, |
||||
|
{ "Bind ", s_bind }, |
||||
|
{ "Joy usb ", s_usb}, |
||||
|
{ "Joy calib ", s_joy }, |
||||
|
{ "HF calib ", NULL }, |
||||
|
{ " ", NULL }, |
||||
|
}; |
||||
|
|
||||
|
bool wait = false; |
||||
|
char curr[2][16]; |
||||
|
snprintf(curr[0], sizeof(curr[0]), "> %s", menus[this->curr_selected].name); |
||||
|
snprintf(curr[1], sizeof(curr[1]), " %s", menus[this->curr_selected+1].name); |
||||
|
|
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print(curr[0]); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(curr[1]); |
||||
|
|
||||
|
input.update(); |
||||
|
if (false == input.is_centered(Input::MENU_UP_DOWN)) { |
||||
|
if (input.is_low(Input::MENU_UP_DOWN)){ |
||||
|
this->curr_selected +=1; |
||||
|
if ( this->curr_selected > 3) |
||||
|
this->curr_selected = 3; |
||||
|
else |
||||
|
wait = true; |
||||
|
} |
||||
|
if (input.is_high(Input::MENU_UP_DOWN)){ |
||||
|
this->curr_selected -=1; |
||||
|
if ( this->curr_selected < 0) |
||||
|
this->curr_selected = 0; |
||||
|
else |
||||
|
wait = true; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
if (input.is_high(Input::MENU_LEFT_RIGHT)) { |
||||
|
lcd.setCursor(0,0); |
||||
|
lcd.print("entering "); |
||||
|
lcd.setCursor(0,1); |
||||
|
lcd.print(menus[this->curr_selected].name); |
||||
|
// do wait until its centered
|
||||
|
while(false == input.is_centered(Input::MENU_LEFT_RIGHT)) { |
||||
|
input.update(); |
||||
|
} |
||||
|
|
||||
|
if (menus[this->curr_selected].state != NULL) { |
||||
|
new_state = menus[this->curr_selected].state; |
||||
|
} |
||||
|
wait = true; |
||||
|
} |
||||
|
|
||||
|
if(wait) |
||||
|
delay(500); |
||||
|
} |
||||
|
|
||||
|
void LCD_state_menu::leave(void) |
||||
|
{ |
||||
|
lcd.clear(); |
||||
|
} |
||||
@ -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 <http://www.gnu.org/licenses/>.
|
||||
|
*/ |
||||
|
//**************************
|
||||
|
// 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;i<len-2;i++) |
||||
|
pktt[i]=pkt[i]; // Buffer telemetry values to be sent
|
||||
|
|
||||
|
if(pktt[6]>0 && 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 **/ |
||||
|
/**************************/ |
||||
|
/**************************/ |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue