From aeb4e0cbd17c61e135349af837c14e074edfa248 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Wed, 27 Mar 2019 21:38:08 +0100 Subject: [PATCH 01/10] improve battery charger plug --- 3d/quad_charge_plug.scad | 54 +++++++++++++++++++++++----------------- 3d/stick.scad | 14 +++++++---- 2 files changed, 40 insertions(+), 28 deletions(-) diff --git a/3d/quad_charge_plug.scad b/3d/quad_charge_plug.scad index 5418632..c480034 100644 --- a/3d/quad_charge_plug.scad +++ b/3d/quad_charge_plug.scad @@ -4,36 +4,44 @@ use ; module plug_surface () { projection() translate([0,0,10]) - intersection() { plug(); - translate([-10,-10,-10]) - ;//cube([20,20,10]); } } module plug() { - - rotate([-90,0,0]) + rotate([270,0,0]) import("quad_plug.stl"); } -translate([0,0,-10+10*$t]) -rotate([180,0,180]) -%plug() ; -difference () { - aligned_cube([8,5,5]); - union() { - translate([0,0,-0.1]) - scale([1.05,1.1,1]) - linear_extrude(4) - plug_surface(); - } +module charger_plug() { + difference () { + // hull + aligned_cube([9,6,5]); + + // plug cut + translate([0,0,1]) + union() { + scale([1.2,1.3,1]) + linear_extrude(5) + plug_surface(); + } - // pin cuts - for (i= [1,-1]) { - translate([1*i,0,0]) - aligned_cube([1,1,50]); - translate([1*i,0,-1]) - aligned_cube([2,2,5]); + // pin cuts + for (i= [1,-1]) { + translate([1*i,0,-0.01]) + aligned_cube([0.85,0.85,50]); + translate([1*i,0,1]) + aligned_cube([2,2,5]); + } } -} \ No newline at end of file + translate([0,1.9,-0.01]) + aligned_cube([9,2,5]); +} + + + +translate([0,0,5+10*$t]) +rotate([0,0,0]) +%plug() ; + +charger_plug(); \ No newline at end of file diff --git a/3d/stick.scad b/3d/stick.scad index b9492b8..ef54acc 100644 --- a/3d/stick.scad +++ b/3d/stick.scad @@ -7,9 +7,9 @@ use // draw itself stick(); -module stick(h=10,is_ps2_shaft=0) +module stick(h=10,is_ps2_shaft=1) { - $fn=32; + $fn=128; translate([0,0,15-3]) { cylinder(d=5,h=h); translate([0,0,h]) @@ -22,7 +22,7 @@ module stick(h=10,is_ps2_shaft=0) } // dust protector - translate([0,0,-3+2.9]) { + translate([0,0,0]) { resize([30,30,22]) protector_cone(thickness=0.4*2.5); } @@ -77,13 +77,17 @@ module stick_mount(is_ps2_shaft=0) { difference() { color([1,0,1]) { - translate([0,0,5]) - cylinder_flange_sphere($fn=32,r2=5, r1=3, h=5); + translate([0,0,4]) + cylinder_flange_sphere($fn=32,r2=8, r1=3, h=4.5); + translate([0,0,8.5]) + cylinder_flange_sphere($fn=32,r2=3, r1=8, h=4.5); + if ( is_ps2_shaft ) { cylinder(d=6.75,h=10); } else { cylinder(d=6,h=10); } + //cylinder(d=15,h=0.1); } translate([0,0,-eps]) if ( is_ps2_shaft ) { From c1d2b9b0f261e7e0184032ae4420b30a8abe42d6 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Wed, 27 Mar 2019 21:38:35 +0100 Subject: [PATCH 02/10] Partially rework input/eeprom + further cleanup --- remote/include/FrSkyD_cc2500.h | 4 +- remote/include/Multiprotocol.h | 1 - remote/include/common.h | 12 --- remote/include/crc.h | 7 ++ remote/include/eeprom.h | 92 +++++----------- remote/include/input.h | 48 +++++---- remote/src/FrSkyD_cc2500.cpp | 48 +++++++-- remote/src/Multiprotocol.cpp | 1 - remote/src/common.cpp | 117 -------------------- remote/src/crc.cpp | 22 ++++ remote/src/eeprom.cpp | 102 ++++++++++++++++++ remote/src/input.cpp | 191 +++++++++++++++++---------------- remote/src/state.cpp | 7 +- 13 files changed, 329 insertions(+), 323 deletions(-) create mode 100644 remote/include/crc.h create mode 100644 remote/src/crc.cpp create mode 100644 remote/src/eeprom.cpp diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h index da8c7ee..b45ca4d 100644 --- a/remote/include/FrSkyD_cc2500.h +++ b/remote/include/FrSkyD_cc2500.h @@ -17,6 +17,7 @@ #define _FRSKYD_CC2500_H_ #include +void Frsky_init_hop(void); void frsky2way_init(uint8_t bind); void frsky2way_build_bind_packet(); @@ -26,6 +27,5 @@ void frsky2way_data_frame(void); uint16_t initFrSky_2way(void); uint16_t ReadFrSky_2way(void); - - +uint16_t convert_channel_frsky(uint8_t num); #endif diff --git a/remote/include/Multiprotocol.h b/remote/include/Multiprotocol.h index 0ae59e9..52c236b 100644 --- a/remote/include/Multiprotocol.h +++ b/remote/include/Multiprotocol.h @@ -33,7 +33,6 @@ extern uint8_t protocol_flags; extern uint8_t protocol_flags2; extern uint8_t pkt[MAX_PKT];//telemetry receiving packets extern uint8_t prev_option; -extern uint8_t hopping_frequency[50]; extern uint8_t crc8; extern uint8_t packet_count; extern uint8_t RX_num; diff --git a/remote/include/common.h b/remote/include/common.h index 0a95036..76b9a0b 100644 --- a/remote/include/common.h +++ b/remote/include/common.h @@ -19,17 +19,6 @@ #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 **/ @@ -44,7 +33,6 @@ enum { FRSKY_DATA5 }; -void Frsky_init_hop(void); /******************************/ /** FrSky V, D and X routines **/ /******************************/ diff --git a/remote/include/crc.h b/remote/include/crc.h new file mode 100644 index 0000000..090f388 --- /dev/null +++ b/remote/include/crc.h @@ -0,0 +1,7 @@ +#ifndef __CRC_H_H__ +#define __CRC_H_H__ +#include + +uint32_t crc_update (uint32_t crc, uint8_t data); + +#endif diff --git a/remote/include/eeprom.h b/remote/include/eeprom.h index 0a43387..188b25a 100644 --- a/remote/include/eeprom.h +++ b/remote/include/eeprom.h @@ -1,81 +1,37 @@ #ifndef _EEPROM_H_ #define _EEPROM_H_ -#include -#include -#include -#include "tx_def.h" +#include "input.h" +extern class Eeprom_config eeprom_config; -#define CURRENT_VERSION 0x01 -struct eeprom_data_v1 { - uint8_t version; - uint32_t crc; - struct { - struct { - uint16_t max; - uint16_t min; - uint8_t inverted; - } throttle, yaw, roll, pitch, aux[5]; - uint32_t master_id; - } data; - -}; +class Eeprom_config { +public: + Eeprom_config(void); + ~Eeprom_config(void); -const static uint32_t crc_table[16] = { - 0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac, - 0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c, - 0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c, - 0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c -}; + int validate(void); + int read(void); + int write(void); -uint32_t crc_update (uint32_t crc, uint8_t data) -{ - uint8_t tbl_idx; + int get_ch_config(struct Input::ch_config* config); + int set_ch_config(struct Input::ch_config* config); - tbl_idx = crc ^ (data >> (0 * 4)); - crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); + int get_master_id(uint32_t* master_id); + int set_master_id(uint32_t master_id); - tbl_idx = crc ^ (data >> (1 * 4)); - crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); - - return crc; +private: + #define CURRENT_VERSION 0x01 + struct eeprom_data_v1 { + uint8_t version; + uint32_t data_crc; + struct { + uint32_t master_id; + struct Input::ch_config[CH_COUNT]; + } data; + } current_config; + bool sucessfull_read; } -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 diff --git a/remote/include/input.h b/remote/include/input.h index 22484e2..53e4ef7 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -5,8 +5,7 @@ #define NUM_TX_CHN 16 -extern uint16_t Channel_data[NUM_TX_CHN]; -extern uint16_t Failsafe_data[NUM_TX_CHN]; +extern Input input; extern const char* ch_name[NUM_TX_CHN]; class Input { public: @@ -29,20 +28,17 @@ class Input { MENU_UP_DOWN = CH_PITCH, MENU_LEFT_RIGHT = CH_ROLL, }; - - struct data { - uint16_t ch_data[CH_COUNT]; - bool menu; + struct ch_config { + uint16_t max; + uint16_t min; + bool inverted; + bool is_analog; }; - Input(void); void init(void); - void do_calibration(void); void update(void); - struct data* get_curr_input(void); - struct data* get_old_input(void); void update_inputs(void); void mark_processed(void); @@ -55,25 +51,37 @@ class Input { void invert_ch(enum input_channels ch); void print_ch(enum input_channels ch); - void calibration_init(void); + + void calibration_reset(void); bool calibration_update(void); + + void set_calibration(struct ch_config *new_config); + void get_calibration(struct ch_config *curr_config); + + uint16_t *get_channel_data(void); private: + // raw sticks input + uint16_t ch_raw[CH_COUNT]; + + // calculated inputs (my be inverted + struct data { + uint16_t ch_data[CH_COUNT]; + bool menu; + }; + + // actual tx channel data + uint16_t channel_data[CH_COUNT]; + struct data input[2]; struct data* curr; struct data* old; - uint16_t ch_raw[CH_COUNT]; - struct { - uint16_t max; - uint16_t min; - bool inverted; - bool is_analog; - } ch_config[CH_COUNT]; + // config of each channel + struct ch_config ch_config[CH_COUNT]; + // pin setttings uint32_t pins[CH_COUNT]; }; -extern uint16_t Channel_data[NUM_TX_CHN]; -extern Input input; #endif diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp index 3aa90d4..9882b76 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -17,7 +17,9 @@ #include "cc2500_spi.h" #include "Multiprotocol.h" +uint8_t hopping_frequency[50]; uint16_t counter; + void frsky2way_init(uint8_t bind) { //debugln("frsky2way_init"); @@ -80,17 +82,13 @@ void frsky2way_data_frame() packet[11] = 0; packet[16] = 0; packet[17] = 0; - for(uint8_t i = 0; i < 8; i++) - { + for(uint8_t i = 0; i < 8; i++) { uint16_t value; - value = convert_channel_frsky(i); - if(i < 4) - { + value = convert_channel_frsky(i); + if(i < 4) { packet[6+i] = value & 0xff; packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01)); - } - else - { + } else { packet[8+i] = value & 0xff; packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01)); } @@ -132,7 +130,7 @@ uint16_t ReadFrSky_2way() // menu tells us to stop so do not inc state //state++; } - + if(state == FRSKY_BIND_DONE) { debugln("%s bind done fr",__func__); } @@ -200,3 +198,35 @@ uint16_t ReadFrSky_2way() } return state == FRSKY_DATA4 ? 7500 : 9000; } + +uint16_t convert_channel_frsky(uint8_t num) +{ + uint16_t val = input.get_channel_data()[num]; + return ((val * 15) >> 4) + 1290; +} + +void Frsky_init_hop(void) +{ + uint8_t val; + uint8_t channel = rx_tx_addr[0] & 0x07; + uint8_t channel_spacing = rx_tx_addr[1]; + //Filter bad tables + if (channel_spacing < 0x02) + channel_spacing += 0x02; + if (channel_spacing > 0xE9) + channel_spacing -= 0xE7; + if (channel_spacing % 0x2F == 0) + channel_spacing++; + + hopping_frequency[0] = channel; + + for (uint8_t i = 1; i < 50; i++) { + channel = (channel + channel_spacing) % 0xEB; + val = channel; + + if ((val == 0x00) || (val == 0x5A) || (val == 0xDC)) + val++; + + hopping_frequency[i] = i > 46 ? 0 : val; + } +} diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index f1b9d6a..1e9c900 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -66,7 +66,6 @@ uint16_t packet_period; uint8_t packet_count; uint8_t packet_sent; uint8_t packet_length; -uint8_t hopping_frequency[50]; uint8_t *hopping_frequency_ptr; uint8_t hopping_frequency_no=0; uint8_t rf_ch_num; diff --git a/remote/src/common.cpp b/remote/src/common.cpp index 88606a7..0235c52 100644 --- a/remote/src/common.cpp +++ b/remote/src/common.cpp @@ -21,128 +21,11 @@ #include "input.h" -/************************/ -/** Convert routines **/ -/************************/ -// Revert a channel and store it -void reverse_channel(uint8_t num) -{ - uint16_t val = 2048 - Channel_data[num]; - if (val >= 2048) - val = 2047; - Channel_data[num] = val; -} -// Channel value is converted to ppm 860<->2140 -125%<->+125% and 988<->2012 -100%<->+100% -uint16_t convert_channel_ppm(uint8_t num) -{ - uint16_t val = Channel_data[num]; - return (((val << 2) + val) >> 3) + 860; //value range 860<->2140 -125%<->+125% -} -// Channel value 100% is converted to 10bit values 0<->1023 -uint16_t convert_channel_10b(uint8_t num) -{ - uint16_t val = Channel_data[num]; - val = ((val << 2) + val) >> 3; - if (val <= 128) - return 0; - if (val >= 1152) - return 1023; - return val - 128; -} -// Channel value 100% is converted to 8bit values 0<->255 -uint8_t convert_channel_8b(uint8_t num) -{ - uint16_t val = Channel_data[num]; - val = ((val << 2) + val) >> 5; - if (val <= 32) - return 0; - if (val >= 288) - return 255; - return val - 32; -} - -// Channel value 100% is converted to value scaled -int16_t convert_channel_16b_limit(uint8_t num, int16_t min, int16_t max) -{ - int32_t val = limit_channel_100(num); // 204<->1844 - val = (val - CHANNEL_MIN_100) * (max - min) / (CHANNEL_MAX_100 - CHANNEL_MIN_100) + min; - return (uint16_t)val; -} - -// Channel value -125%<->125% is scaled to 16bit value with no limit -int16_t convert_channel_16b_nolimit(uint8_t num, int16_t min, int16_t max) -{ - int32_t val = Channel_data[num]; // 0<->2047 - val = (val - CHANNEL_MIN_100) * (max - min) / (CHANNEL_MAX_100 - CHANNEL_MIN_100) + min; - return (uint16_t)val; -} - -// Channel value is converted sign + magnitude 8bit values -uint8_t convert_channel_s8b(uint8_t num) -{ - uint8_t ch; - ch = convert_channel_8b(num); - return (ch < 128 ? 127 - ch : ch); -} - -// Channel value is limited to 100% -uint16_t limit_channel_100(uint8_t num) -{ - if (Channel_data[num] >= CHANNEL_MAX_100) - return CHANNEL_MAX_100; - if (Channel_data[num] <= CHANNEL_MIN_100) - return CHANNEL_MIN_100; - return Channel_data[num]; -} - -// Channel value is converted for HK310 -void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high) -{ - uint16_t temp = 0xFFFF - (3440 + ((Channel_data[num] * 5) >> 1)) / 3; - *low = (uint8_t)(temp & 0xFF); - *high = (uint8_t)(temp >> 8); -} - -// Failsafe value is converted for HK310 -void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high) -{ - uint16_t temp = 0xFFFF - (3440 + ((Failsafe_data[num] * 5) >> 1)) / 3; - *low = (uint8_t)(temp & 0xFF); - *high = (uint8_t)(temp >> 8); -} - -// Channel value for FrSky (PPM is multiplied by 1.5) -uint16_t convert_channel_frsky(uint8_t num) -{ - uint16_t val = Channel_data[num]; - return ((val * 15) >> 4) + 1290; -} - /******************************/ /** FrSky D and X routines **/ /******************************/ #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) -void Frsky_init_hop(void) -{ - uint8_t val; - uint8_t channel = rx_tx_addr[0] & 0x07; - uint8_t channel_spacing = rx_tx_addr[1]; - //Filter bad tables - if (channel_spacing < 0x02) channel_spacing += 0x02; - if (channel_spacing > 0xE9) channel_spacing -= 0xE7; - if (channel_spacing % 0x2F == 0) channel_spacing++; - - hopping_frequency[0] = channel; - for (uint8_t i = 1; i < 50; i++) - { - channel = (channel + channel_spacing) % 0xEB; - val = channel; - if ((val == 0x00) || (val == 0x5A) || (val == 0xDC)) - val++; - hopping_frequency[i] = i > 46 ? 0 : val; - } -} #endif /******************************/ /** FrSky V, D and X routines **/ diff --git a/remote/src/crc.cpp b/remote/src/crc.cpp new file mode 100644 index 0000000..0efd817 --- /dev/null +++ b/remote/src/crc.cpp @@ -0,0 +1,22 @@ +#include "Arduino.h" + +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; + uint8_t mask = 0x0f; + + tbl_idx = crc ^ (data >> (0 * 4)); + crc = (crc_table + (tbl_idx & mask)) ^ (crc >> 4); + + tbl_idx = crc ^ (data >> (1 * 4)); + crc = (crc_table + (tbl_idx & mask)) ^ (crc >> 4); + + return crc; +} diff --git a/remote/src/eeprom.cpp b/remote/src/eeprom.cpp new file mode 100644 index 0000000..90ea84c --- /dev/null +++ b/remote/src/eeprom.cpp @@ -0,0 +1,102 @@ +#include + +#include +#include +#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() { + if (this->sucessfull_read == false) { + return -1; + } + + if (this->current_config.version != CURRENT_VERSION) { + return -1; + } + uint32_t crc_calc = 0; + + uint8_t * data = &(this->current_config.data); + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i = 0) { + crc_calc = crc_update(crc_calc, data[i]); + } + + if (crc_calc != this->current_config.data_crc) { + return -1; + } + return 0; +} + +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 = 0) { + EEPROM.read(0x10 + i , data); + data+=1; + } + + this->sucessfull_read = true; + return 0; +} + +int Eeprom_config::write(void) { + uint8_t *data = NULL; + data = (uint8_t *) &this->current_config; + + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i = 0) { + EEPROM.write(0x10 + i , data); + data+=1; + } + + return 0; +} + +int Eeprom_config::get_ch_config(struct ch_config* config) { + if (this->sucessfull_read == false && config != NULL) { + return -1; + } + memcpy(config, this->current_config.data.ch, sizeof(struct ch_config) * CH_COUNT); + return 0; +} + +int Eeprom_config::set_ch_config(struct ch_config* config) { + if (this->sucessfull_read == false && config != NULL) { + return -1; + } + memcpy(this->current_config.data.ch, config, sizeof(struct ch_config) * 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; +} diff --git a/remote/src/input.cpp b/remote/src/input.cpp index 807303f..8ef16fa 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -8,8 +8,6 @@ #include "state.h" Input input; -uint16_t Channel_data[NUM_TX_CHN]; -uint16_t Failsafe_data[NUM_TX_CHN]; const char* ch_name[NUM_TX_CHN] = { "CH_ROLL", @@ -29,16 +27,21 @@ Input::Input(void) { this->curr = &(this->input[0]); this->old = &(this->input[1]); memset(this->input,0, sizeof(this->input)); - //InitFailsafe - for (uint8_t i = 0; i < NUM_TX_CHN; i++) - Failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; - Failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% - // init channel + //InitFailsafe + for (uint8_t i = 0; i < NUM_TX_CHN; i++) + Failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; + Failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% - for (uint8_t i = 0; i < NUM_TX_CHN; i++) - Channel_data[i] = 1024; - Channel_data[CH_THROTTLE] = 204; + // init channel + + for (uint8_t i = 0; i < NUM_TX_CHN; i++) + this->channel_data[i] = 1024; + this->channel_data[CH_THROTTLE] = 204; +} + +uint16_t *Input::get_channel_data(void) { + return this->channel_data; } void Input::mark_processed(void) { struct data* temp = this->old; @@ -55,71 +58,67 @@ struct Input::data* Input::get_old_input(void) { } void Input::init() { - this->pins[CH_THROTTLE] = Throttle_pin; - this->ch_config[CH_THROTTLE].is_analog = true; + this->pins[CH_THROTTLE] = Throttle_pin; + this->ch_config[CH_THROTTLE].is_analog = true; - this->pins[CH_YAW] = Yaw_pin; - this->ch_config[CH_YAW].is_analog = true; + this->pins[CH_YAW] = Yaw_pin; + this->ch_config[CH_YAW].is_analog = true; - this->pins[CH_PITCH] = Pitch_pin; - this->ch_config[CH_PITCH].is_analog = true; + this->pins[CH_PITCH] = Pitch_pin; + this->ch_config[CH_PITCH].is_analog = true; - this->pins[CH_ROLL] = Roll_pin; - this->ch_config[CH_ROLL].is_analog = true; + this->pins[CH_ROLL] = Roll_pin; + this->ch_config[CH_ROLL].is_analog = true; - this->pins[CH_AUX1] = Aux1_pin; - this->ch_config[CH_AUX1].is_analog = false; + this->pins[CH_AUX1] = Aux1_pin; + this->ch_config[CH_AUX1].is_analog = false; - this->pins[CH_AUX2] = Aux2_pin; - this->ch_config[CH_AUX2].is_analog = false; + this->pins[CH_AUX2] = Aux2_pin; + this->ch_config[CH_AUX2].is_analog = false; - this->pins[CH_AUX3] = Aux3_pin; - this->ch_config[CH_AUX3].is_analog = false; + this->pins[CH_AUX3] = Aux3_pin; + this->ch_config[CH_AUX3].is_analog = false; - this->pins[CH_AUX4] = Aux4_pin; - this->ch_config[CH_AUX4].is_analog = false; + this->pins[CH_AUX4] = Aux4_pin; + this->ch_config[CH_AUX4].is_analog = false; - this->pins[CH_AUX5] = Aux5_pin; - this->ch_config[CH_AUX5].is_analog = false; + this->pins[CH_AUX5] = Aux5_pin; + this->ch_config[CH_AUX5].is_analog = false; - this->pins[CH_AUX6] = Aux6_pin; - this->ch_config[CH_AUX6].is_analog = false; + this->pins[CH_AUX6] = Aux6_pin; + this->ch_config[CH_AUX6].is_analog = false; - for (uint8_t i = 0; i < CH_COUNT; ++i) { - pinMode(this->pins[i], INPUT); - } - pinMode(Menu_pin,INPUT); - - //analogReadResolution(16); + for (uint8_t i = 0; i < CH_COUNT; ++i) { + pinMode(this->pins[i], INPUT); + } + pinMode(Menu_pin,INPUT); - // move this to eeprom later - this->ch_config[CH_THROTTLE].inverted = false; - this->ch_config[CH_YAW].inverted = false; - this->ch_config[CH_ROLL].inverted = false; - this->ch_config[CH_PITCH].inverted = false; - this->ch_config[CH_AUX1].inverted = false; - this->ch_config[CH_AUX2].inverted = false; - this->ch_config[CH_AUX3].inverted = false; - this->ch_config[CH_AUX4].inverted = false; - this->ch_config[CH_AUX5].inverted = false; + //analogReadResolution(16); -} -void Input::do_calibration(void) { + // move this to eeprom later + this->ch_config[CH_THROTTLE].inverted = false; + this->ch_config[CH_YAW].inverted = false; + this->ch_config[CH_ROLL].inverted = false; + this->ch_config[CH_PITCH].inverted = false; + this->ch_config[CH_AUX1].inverted = false; + this->ch_config[CH_AUX2].inverted = false; + this->ch_config[CH_AUX3].inverted = false; + this->ch_config[CH_AUX4].inverted = false; + this->ch_config[CH_AUX5].inverted = false; } bool Input::is_centered(void) { - return - this->is_centered(CH_ROLL) && - this->is_centered(CH_PITCH) && - this->is_centered(CH_THROTTLE) && - this->is_centered(CH_YAW); + return this->is_centered(CH_ROLL) && + this->is_centered(CH_PITCH) && + this->is_centered(CH_THROTTLE) && + this->is_centered(CH_YAW); } bool Input::is_centered(enum Input::input_channels ch) { - uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; - uint16_t delta = range / 5; + uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; + uint16_t delta = range / 5; - if ( this->curr->ch_data[ch] < this->ch_config[ch].min + range / 2 - delta || + if ( this->curr->ch_data[ch] < this->ch_config[ch].min + range / 2 - delta || this->curr->ch_data[ch] > this->ch_config[ch].min + range / 2 + delta ) { // pitch is not centered @@ -129,18 +128,18 @@ bool Input::is_centered(enum Input::input_channels ch) { } bool Input::is_high(enum Input::input_channels ch) { - uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; - uint16_t delta = range / 5; - if ( this->curr->ch_data[ch] > this->ch_config[ch].max - delta) { + uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; + uint16_t delta = range / 5; + if ( this->curr->ch_data[ch] > this->ch_config[ch].max - delta) { return true; } return false; } bool Input::is_low(enum Input::input_channels ch) { - uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; - uint16_t delta = range / 5; - if ( this->curr->ch_data[ch] < this->ch_config[ch].min + delta) { + uint16_t range = this->ch_config[ch].max - this->ch_config[ch].min; + uint16_t delta = range / 5; + if ( this->curr->ch_data[ch] < this->ch_config[ch].min + delta) { return true; } return false; @@ -190,49 +189,57 @@ bool Input::calibration_update(void) { return changed; } -void Input::calibration_init(void) { +void Input::calibration_reset(void) { for (uint8_t ch = 0; ch < CH_COUNT; ch++) { this->ch_config[ch].min = this->ch_raw[ch]; this->ch_config[ch].max = this->ch_raw[ch]; } } -void Input::update(void) { - - for (uint8_t ch = 0; ch < CH_MAX; ch ++) { - - if (this->ch_config[ch].is_analog) - this->ch_raw[ch] = analogRead(this->pins[ch]); - else - this->ch_raw[ch] = digitalRead(this->pins[ch]) == HIGH; +void Input::set_calibration(struct ch_config *new_config) +{ + if (new_config != NULL) { + memcpy(this->ch_config, new_config, sizeof(ch_config)); + } +} +void Input::get_calibration(struct ch_config *curr_config) +{ + if (curr_config != NULL) { + memcpy(curr_config, this->ch_config, sizeof(ch_config)); + } +} - // do inverting - if (this->ch_config[ch].inverted) - this->curr->ch_data[ch] = this->ch_config[ch].max - this->ch_raw[ch]; - else - this->curr->ch_data[ch] = this->ch_raw[ch]; +void Input::update(void) { + for (uint8_t ch = 0; ch < CH_MAX; ch ++) { + + if (this->ch_config[ch].is_analog) + this->ch_raw[ch] = analogRead(this->pins[ch]); + else + this->ch_raw[ch] = digitalRead(this->pins[ch]) == HIGH; + + // do inverting + if (this->ch_config[ch].inverted) + this->curr->ch_data[ch] = this->ch_config[ch].max - this->ch_raw[ch]; + else + this->curr->ch_data[ch] = this->ch_raw[ch]; + + // cap on max + if (this->ch_config[ch].min > this->curr->ch_data[ch]) { + this->curr->ch_data[ch] = this->ch_config[ch].min; + } else if (this->ch_config[ch].max < this->curr->ch_data[ch]) { + this->curr->ch_data[ch] = this->ch_config[ch].max; + } + } + this->curr->menu = digitalRead(Menu_pin) == HIGH; - // cap on max - if (this->ch_config[ch].min > this->curr->ch_data[ch]) { - this->curr->ch_data[ch] = this->ch_config[ch].min; - } else if (this->ch_config[ch].max < this->curr->ch_data[ch]) { - this->curr->ch_data[ch] = this->ch_config[ch].max; + for (uint8_t ch = 0; ch < CH_COUNT; ++ch) { + this->channel_data[ch] = map(this->curr->ch_data[ch], this->ch_config[ch].min, this->ch_config[ch].max, CHANNEL_MAX_100, CHANNEL_MIN_100); } - } - this->curr->menu = digitalRead(Menu_pin) == HIGH; - /*debug_input("t%d y%d r%d p%d a1_%d a2_%d a3_%d a4_%d a5_%d m%d", this->curr->throttle,this->curr->yaw,this->curr->roll,this->curr->pitch, this->curr->aux[0],this->curr->aux[1],this->curr->aux[2],this->curr->aux[3], this->curr->aux[4],this->curr->aux[5],this->curr->menu );*/ - - // only do channeloutpu if needed - if (curr_state != s_fly) - return; - - for (uint8_t ch = 0; ch < CH_COUNT; ++ch) { - Channel_data[ch] = map(this->curr->ch_data[ch], this->ch_config[ch].min, this->ch_config[ch].max, CHANNEL_MAX_100, CHANNEL_MIN_100); - } } +// Channel value for FrSky (PPM is multiplied by 1.5) diff --git a/remote/src/state.cpp b/remote/src/state.cpp index eb50493..6f0491b 100644 --- a/remote/src/state.cpp +++ b/remote/src/state.cpp @@ -4,6 +4,7 @@ #include "FrSkyD_cc2500.h" #include "state.h" #include "input.h" +#include "eeprom.h" #include "debug.h" #include "tx_def.h" @@ -212,7 +213,7 @@ void LCD_state_joy_calibration::update(void) { int8_t i; // init min/max - input.calibration_init(); + input.calibration_reset(); // min max calibration lcd.setCursor(0,0); @@ -274,6 +275,10 @@ void LCD_state_joy_calibration::update(void) { } } + struct ch_config ch_config[CH_COUNT]; + input.get_calibration(ch_config); + + lcd.setCursor(0,0); lcd.print("center again "); lcd.setCursor(0,1); From ea1741d7b7d319b71f589ebd6a29cf077547bc87 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Tue, 2 Apr 2019 23:05:47 +0200 Subject: [PATCH 03/10] Partially rework input/eeprom + further cleanup part2 --- remote/include/FrSkyD_cc2500.h | 1 + remote/include/Multiprotocol.h | 4 +- remote/include/Validate.h | 10 - remote/include/config.h | 79 +------- remote/include/crc.h | 10 +- remote/include/eeprom.h | 4 +- remote/include/input.h | 7 +- remote/include/state.h | 1 + remote/include/telemetry.h | 361 +++++++++++++++++++++++++++++++++ remote/include/tx_def.h | 36 ---- remote/src/FrSkyD_cc2500.cpp | 55 ++--- remote/src/Multiprotocol.cpp | 222 +++----------------- remote/src/cc2500_spi.cpp | 4 +- remote/src/crc.cpp | 28 ++- remote/src/eeprom.cpp | 25 ++- remote/src/input.cpp | 14 +- remote/src/state.cpp | 296 --------------------------- remote/src/state_bind.cpp | 77 +++++++ remote/src/state_fly.cpp | 29 +++ remote/src/state_init.cpp | 31 +++ remote/src/state_joy_calib.cpp | 115 +++++++++++ remote/src/state_menu.cpp | 84 ++++++++ 22 files changed, 802 insertions(+), 691 deletions(-) create mode 100644 remote/include/telemetry.h create mode 100644 remote/src/state_bind.cpp create mode 100644 remote/src/state_fly.cpp create mode 100644 remote/src/state_init.cpp create mode 100644 remote/src/state_joy_calib.cpp create mode 100644 remote/src/state_menu.cpp diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h index b45ca4d..09417c3 100644 --- a/remote/include/FrSkyD_cc2500.h +++ b/remote/include/FrSkyD_cc2500.h @@ -26,6 +26,7 @@ void frsky2way_data_frame(void); uint16_t initFrSky_2way(void); uint16_t ReadFrSky_2way(void); +uint16_t ReadFrSky_2way_bind(void); uint16_t convert_channel_frsky(uint8_t num); #endif diff --git a/remote/include/Multiprotocol.h b/remote/include/Multiprotocol.h index 52c236b..a917028 100644 --- a/remote/include/Multiprotocol.h +++ b/remote/include/Multiprotocol.h @@ -74,9 +74,7 @@ enum MultiPacketTypes #define CHANGE_PROTOCOL_FLAG_off protocol_flags &= ~_BV(1) #define IS_CHANGE_PROTOCOL_FLAG_on ( ( protocol_flags & _BV(1) ) !=0 ) // -#define POWER_FLAG_on protocol_flags |= _BV(2) -#define POWER_FLAG_off protocol_flags &= ~_BV(2) -#define IS_POWER_FLAG_on ( ( protocol_flags & _BV(2) ) !=0 ) +#define IS_POWER_FLAG_on (true) // #define RANGE_FLAG_on protocol_flags |= _BV(3) #define RANGE_FLAG_off protocol_flags &= ~_BV(3) diff --git a/remote/include/Validate.h b/remote/include/Validate.h index 6969b55..6b85fd8 100644 --- a/remote/include/Validate.h +++ b/remote/include/Validate.h @@ -20,14 +20,4 @@ #endif #endif - -#if defined(ENABLE_BIND_CH) - #if BIND_CH<4 - #error BIND_CH must be above 4. - #endif - #if BIND_CH>16 - #error BIND_CH must be below or equal to 16. - #endif -#endif - #endif diff --git a/remote/include/config.h b/remote/include/config.h index e903306..59c8e24 100644 --- a/remote/include/config.h +++ b/remote/include/config.h @@ -16,17 +16,8 @@ #define _CONFIG_H_ #include -/**********************************************/ -/** Multiprotocol module configuration file ***/ -/**********************************************/ - -/*******************/ -/*** TX SETTINGS ***/ -/*******************/ -//Modify the channel order based on your TX: AETR, TAER, RETA... -//Examples: Flysky & DEVO is AETR, JR/Spektrum radio is TAER, Multiplex is AERT... -//Default is AETR. -#define AETR + +#define DEFAULT_BIND_TIME 13 /*seconds*/ /*****************/ /*** AUTO BIND ***/ // Also referred as "Bind on powerup" @@ -171,37 +162,6 @@ /*************************/ /*** PPM MODE SETTINGS ***/ /*************************/ -//In this section you can configure all details about PPM. -//If you do not plan to use the PPM mode comment this line using "//" to save Flash space, you don't need to configure anything below in this case -#define ENABLE_PPM - -/** TX END POINTS **/ -//It is important for the module to know the endpoints of your radio. -//Below are some standard transmitters already preconfigured. -//Uncomment only the one which matches your transmitter. -#define TX_ER9X //ER9X/ERSKY9X/OpenTX ( 988<->2012 microseconds) -//#define TX_DEVO7 //DEVO (1120<->1920 microseconds) -//#define TX_SPEKTRUM //Spektrum (1100<->1900 microseconds) -//#define TX_HISKY //HISKY (1120<->1920 microseconds) -//#define TX_MPX //Multiplex MC2020 (1250<->1950 microseconds) -//#define TX_WALKERA //Walkera PL0811-01H (1000<->1800 microseconds) -//#define TX_CUSTOM //Custom - -// The lines below are used to set the end points in microseconds if you have selected TX_CUSTOM. -// A few things to consider: -// - If you put too big values compared to your TX you won't be able to reach the extremes which is bad for throttle as an example -// - If you put too low values you won't be able to use your full stick range, it will be maxed out before reaching the ends -// - Centered stick value is usually 1500. It should match the middle between MIN and MAX, ie Center=(MAX+MIN)/2. If your TX is not centered you can adjust the value MIN or MAX. -// - 100% is referred as the value when the TX is set to default with no trims - -/** Number of PPM Channels **/ -// The line below is used to set the minimum number of channels which the module should receive to consider a PPM frame valid. -// The default value is 4 to receive at least AETR for flying models but you could also connect the PPM from a car radio which has only 3 channels by changing this number to 3. -#define MIN_PPM_CHANNELS 4 -// The line below is used to set the maximum number of channels which the module should work with. Any channels received above this number are discarded. -// The default value is 16 to receive all possible channels but you might want to filter some "bad" channels from the PPM frame like the ones above 6 on the Walkera PL0811. -#define MAX_PPM_CHANNELS 16 - #define NBR_BANKS 1 extern uint8_t curr_bank; @@ -228,47 +188,12 @@ struct PPM_Parameters enum PROTOCOLS { MODE_SERIAL = 0, // Serial commands - PROTO_FLYSKY = 1, // =>A7105 - PROTO_HUBSAN = 2, // =>A7105 PROTO_FRSKYD = 3, // =>CC2500 - PROTO_HISKY = 4, // =>NRF24L01 - PROTO_V2X2 = 5, // =>NRF24L01 - PROTO_DSM = 6, // =>CYRF6936 - PROTO_DEVO = 7, // =>CYRF6936 - PROTO_YD717 = 8, // =>NRF24L01 - PROTO_KN = 9, // =>NRF24L01 - PROTO_SYMAX = 10, // =>NRF24L01 - PROTO_SLT = 11, // =>NRF24L01 - PROTO_CX10 = 12, // =>NRF24L01 - PROTO_CG023 = 13, // =>NRF24L01 - PROTO_BAYANG = 14, // =>NRF24L01 PROTO_FRSKYX = 15, // =>CC2500 - PROTO_ESKY = 16, // =>NRF24L01 - PROTO_MT99XX = 17, // =>NRF24L01 - PROTO_MJXQ = 18, // =>NRF24L01 - PROTO_SHENQI = 19, // =>NRF24L01 - PROTO_FY326 = 20, // =>NRF24L01 PROTO_SFHSS = 21, // =>CC2500 - PROTO_J6PRO = 22, // =>CYRF6936 - PROTO_FQ777 = 23, // =>NRF24L01 - PROTO_ASSAN = 24, // =>NRF24L01 PROTO_FRSKYV = 25, // =>CC2500 - PROTO_HONTAI = 26, // =>NRF24L01 - PROTO_OPENLRS = 27, // =>OpenLRS hardware - PROTO_AFHDS2A = 28, // =>A7105 - PROTO_Q2X2 = 29, // =>NRF24L01, extension of CX-10 protocol - PROTO_WK2x01 = 30, // =>CYRF6936 - PROTO_Q303 = 31, // =>NRF24L01 - PROTO_GW008 = 32, // =>NRF24L01 - PROTO_DM002 = 33, // =>NRF24L01 - PROTO_CABELL = 34, // =>NRF24L01 - PROTO_ESKY150 = 35, // =>NRF24L01 - PROTO_H8_3D = 36, // =>NRF24L01 PROTO_CORONA = 37, // =>CC2500 - PROTO_CFLIE = 38, // =>NRF24L01 PROTO_HITEC = 39, // =>CC2500 - PROTO_WFLY = 40, // =>CYRF6936 - PROTO_BUGS = 41, // =>A7105 }; enum FRSKYX_SUP_PROTOCOL diff --git a/remote/include/crc.h b/remote/include/crc.h index 090f388..4193f78 100644 --- a/remote/include/crc.h +++ b/remote/include/crc.h @@ -1,7 +1,5 @@ -#ifndef __CRC_H_H__ -#define __CRC_H_H__ -#include - -uint32_t crc_update (uint32_t crc, uint8_t data); - +#ifndef __CRC32_H_ +#define __CRC32_H_ +#include +uint32_t tiny_crc32(const void *data, unsigned int length); #endif diff --git a/remote/include/eeprom.h b/remote/include/eeprom.h index 188b25a..87e466f 100644 --- a/remote/include/eeprom.h +++ b/remote/include/eeprom.h @@ -27,11 +27,11 @@ private: uint32_t data_crc; struct { uint32_t master_id; - struct Input::ch_config[CH_COUNT]; + struct Input::ch_config ch[Input::CH_COUNT]; } data; } current_config; bool sucessfull_read; -} +}; #endif diff --git a/remote/include/input.h b/remote/include/input.h index 53e4ef7..ea5e995 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -5,8 +5,6 @@ #define NUM_TX_CHN 16 -extern Input input; -extern const char* ch_name[NUM_TX_CHN]; class Input { public: enum input_channels { @@ -60,6 +58,7 @@ class Input { uint16_t *get_channel_data(void); private: + // raw sticks input uint16_t ch_raw[CH_COUNT]; @@ -71,17 +70,19 @@ class Input { // actual tx channel data uint16_t channel_data[CH_COUNT]; + uint16_t failsafe_data[CH_COUNT]; struct data input[2]; struct data* curr; struct data* old; - // config of each channel struct ch_config ch_config[CH_COUNT]; // pin setttings uint32_t pins[CH_COUNT]; }; +extern const char* ch_name[NUM_TX_CHN]; +extern Input input; #endif diff --git a/remote/include/state.h b/remote/include/state.h index c05d4db..27371f6 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -6,6 +6,7 @@ #include +extern LiquidCrystal_I2C lcd; void init_state(void); void update_state(void); diff --git a/remote/include/telemetry.h b/remote/include/telemetry.h new file mode 100644 index 0000000..33ce1f7 --- /dev/null +++ b/remote/include/telemetry.h @@ -0,0 +1,361 @@ +/* + 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]; + +#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 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 ; + + 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 ; + } + } +#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/include/tx_def.h b/remote/include/tx_def.h index 0887996..5f646bc 100644 --- a/remote/include/tx_def.h +++ b/remote/include/tx_def.h @@ -1,42 +1,6 @@ #ifndef _TX_DEV_H_ #define _TX_DEV_H_ -// Turnigy PPM and channels -#if defined(TX_ER9X) - #define PPM_MAX_100 2012 // 100% - #define PPM_MIN_100 988 // 100% -#endif - -// Devo PPM and channels -#if defined(TX_DEVO7) - #define PPM_MAX_100 1920 // 100% - #define PPM_MIN_100 1120 // 100% -#endif - -// SPEKTRUM PPM and channels -#if defined(TX_SPEKTRUM) - #define PPM_MAX_100 1900 // 100% - #define PPM_MIN_100 1100 // 100% -#endif - -// HISKY -#if defined(TX_HISKY) - #define PPM_MAX_100 1920 // 100% - #define PPM_MIN_100 1120 // 100% -#endif - -// Multiplex MC2020 -#if defined(TX_MPX) - #define PPM_MAX_100 1950 // 100% - #define PPM_MIN_100 1250 // 100% -#endif - -// Walkera PL0811-01H -#if defined(TX_WALKERA) - #define PPM_MAX_100 1800 // 100% - #define PPM_MIN_100 1000 // 100% -#endif - //Channel MIN MAX values #define CHANNEL_MAX_100 1844 // 100% #define CHANNEL_MIN_100 204 // 100% diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp index 9882b76..73acc6c 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -16,6 +16,8 @@ #include "common.h" #include "cc2500_spi.h" #include "Multiprotocol.h" +#include "input.h" +#include "FrSkyD_cc2500.h" uint8_t hopping_frequency[50]; uint16_t counter; @@ -45,7 +47,7 @@ void frsky2way_build_bind_packet() packet[2] = 0x01; packet[3] = rx_tx_addr[3]; packet[4] = rx_tx_addr[2]; - uint16_t idx = ((state -FRSKY_BIND) % 10) * 5; + uint16_t idx = ((state - FRSKY_BIND) % 10) * 5; packet[5] = idx; packet[6] = hopping_frequency[idx++]; packet[7] = hopping_frequency[idx++]; @@ -111,43 +113,44 @@ uint16_t initFrSky_2way() } return 10000; } - -uint16_t ReadFrSky_2way() +uint16_t ReadFrSky_2way_bind(void) { - //debugln("%s",__func__); + frsky2way_build_bind_packet(); + CC2500_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + CC2500_Strobe(CC2500_SFRX);//0x3A + CC2500_WriteData(packet, packet[0]+1); - if (state < FRSKY_BIND_DONE) { - frsky2way_build_bind_packet(); - CC2500_Strobe(CC2500_SIDLE); - CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); - CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); - CC2500_Strobe(CC2500_SFRX);//0x3A - CC2500_WriteData(packet, packet[0]+1); - if(IS_BIND_DONE) { - state = FRSKY_BIND; - debugln("%s bind done",__func__); - } else { - // menu tells us to stop so do not inc state - //state++; - } + if(IS_BIND_DONE) { + state = FRSKY_BIND; + debugln("%s bind done",__func__); + } else { + state++; + } - if(state == FRSKY_BIND_DONE) { - debugln("%s bind done fr",__func__); - } - return 9000; + if(state == FRSKY_BIND_DONE) { + /* reset state again */ + state = 0; + debugln("%s bind done fr",__func__); } - if (state == FRSKY_BIND_DONE) { + return 9000; +} + +uint16_t ReadFrSky_2way() +{ + if (state <= FRSKY_BIND_DONE) { //debugln("%s bind done",__func__); state = FRSKY_DATA2; frsky2way_init(0); counter = 0; - BIND_DONE; } else if (state == FRSKY_DATA5) { CC2500_Strobe(CC2500_SRX);//0x34 RX enable state = FRSKY_DATA1; return 9200; - } + } + counter = (counter + 1) % 188; if (state == FRSKY_DATA4) { //telemetry receive CC2500_SetTxRxMode(RX_EN); @@ -160,7 +163,7 @@ uint16_t ReadFrSky_2way() if (state == FRSKY_DATA1) { len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - //debugln("%d len",len); + //debugln("%d len",len); if (len && len<=(0x11+3)) { // 20bytes CC2500_ReadData(pkt, len); //received telemetry packets #if defined(TELEMETRY) diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index 1e9c900..797287d 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -51,6 +51,7 @@ uint8_t protocol_flags=0,protocol_flags2=0; uint8_t channel; uint8_t packet[40]; +static void protocol_init(void); uint16_t seed; @@ -85,11 +86,6 @@ uint8_t len; // Mode_select variables uint8_t mode_select; -#ifdef ENABLE_PPM -// PPM variable -volatile uint8_t PPM_chan_max=0; -#endif - #if not defined (ORANGE_TX) && not defined (STM32_BOARD) //Random variable volatile uint32_t gWDT_entropy=0; @@ -120,8 +116,6 @@ void_function_t remote_callback = 0; //forward declarations void modules_reset(); uint32_t random_id(bool create_new); -static void protocol_init(); -uint8_t Update_All(); // Init void setup() @@ -213,7 +207,6 @@ void setup() debugln("Module Id: %lx", MProtocol_id_master); -#ifdef ENABLE_PPM //Protocol and interrupts initialization { uint8_t line=curr_bank*14+mode_select-1; @@ -222,39 +215,30 @@ void setup() sub_protocol = PPM_prot[line].sub_proto; RX_num = PPM_prot[line].rx_num; - debug("protocol: %d ", protocol); - switch(protocol) { - case PROTO_FRSKYD: - debugln("PROTO_FRSKYD"); - break; - case PROTO_FRSKYX: - debugln("PROTO_FRSKYX"); - break; - case PROTO_FRSKYV: - debugln("PROTO_FRSKYV"); - break; - } - debug("sub_protocol: %d\n", sub_protocol); - option = PPM_prot[line].option; // Use radio-defined option value - debug("freq offset: %d\n", option); - if(PPM_prot[line].power) - POWER_FLAG_on; - if(PPM_prot[line].autobind) { - AUTOBIND_FLAG_on; - BIND_IN_PROGRESS; // Force a bind at protocol startup + debug("protocol: %d ", protocol); + switch(protocol) { + case PROTO_FRSKYD: + debugln("PROTO_FRSKYD"); + break; + case PROTO_FRSKYX: + debugln("PROTO_FRSKYX"); + break; + case PROTO_FRSKYV: + debugln("PROTO_FRSKYV"); + break; } + debug("sub_protocol: %d\n", sub_protocol); + option = PPM_prot[line].option; // Use radio-defined option value + debug("freq offset: %d\n", option); line++; protocol_init(); - } -#endif //ENABLE_PPM - debug("Init complete\n"); - input.init(); - input.update(); - init_state(); - + debug("Init complete\n"); + input.init(); + input.update(); + init_state(); } // Main @@ -272,12 +256,6 @@ void loop() return; uint32_t next_callback; - if(remote_callback==0 || IS_WAIT_BIND_on ) { - do { - Update_All(); - } while(remote_callback==0 || IS_WAIT_BIND_on); - } - uint32_t end__ = micros(); uint32_t start = micros(); @@ -310,170 +288,24 @@ void loop() } } -uint8_t Update_All() { - - #ifdef ENABLE_BIND_CH - if(IS_AUTOBIND_FLAG_on && - IS_BIND_CH_PREV_off && - Channel_data[BIND_CH-1] > CHANNEL_MAX_COMMAND && - Channel_data[Input::CH_THROTTLE] < (CHANNEL_MIN_100+50) - ) { // Autobind is on and BIND_CH went up and Throttle is low - CHANGE_PROTOCOL_FLAG_on; //reload protocol - BIND_IN_PROGRESS; //enable bind - - debugln("%s:%d set bind prog",__func__, __LINE__); - BIND_CH_PREV_on; - } - if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]2) - bind_counter=2; - } - #endif //ENABLE_BIND_CH - - input.update(); - - if(IS_CHANGE_PROTOCOL_FLAG_on) { - debugln("%s:%d set bind prog",__func__, __LINE__); - // Protocol needs to be changed or relaunched for bind - protocol_init(); //init new protocol - return 1; - } - return 0; -} - // Protocol start static void protocol_init() { - static uint16_t next_callback; - if(IS_WAIT_BIND_off) { - remote_callback = 0; // No protocol - next_callback=0; // Default is immediate call back - modules_reset(); // Reset all modules - - //Set global ID and rx_tx_addr - MProtocol_id = RX_num + MProtocol_id_master; - set_rx_tx_addr(MProtocol_id); + modules_reset(); // Reset all modules + uint32_t next_callback = 0; - blink=millis(); + //Set global ID and rx_tx_addr + MProtocol_id = RX_num + MProtocol_id_master; + set_rx_tx_addr(MProtocol_id); + debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option); - debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option); - - switch(protocol) // Init the requested protocol - { + switch(protocol) // Init the requested protocol + { case PROTO_FRSKYD: next_callback = initFrSky_2way(); remote_callback = ReadFrSky_2way; break; - } } - - #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH) - if( IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && (cur_protocol[1]&0x80)==0 && mode_select == MODE_SERIAL) { - // Autobind is active but no bind requested by either BIND_CH or BIND. But do not wait if in PPM mode... - WAIT_BIND_on; - return; - } - #endif - WAIT_BIND_off; - CHANGE_PROTOCOL_FLAG_off; - delayMicroseconds(next_callback); - debugln("%s BIND_BUTTON_FLAG_off",__func__); -} - -void update_serial_data() -{ - if(rx_ok_buff[1]&0x20) //check range - RANGE_FLAG_on; - else - RANGE_FLAG_off; - - if(rx_ok_buff[1]&0x40) //check autobind - AUTOBIND_FLAG_on; - else - AUTOBIND_FLAG_off; - - if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high - POWER_FLAG_off; //power low - else - POWER_FLAG_on; //power high - - //Forced frequency tuning values for CC2500 protocols - #if defined(FORCE_FRSKYD_TUNING) - if(protocol==PROTO_FRSKYD) - option=FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD - else - #endif - option=rx_ok_buff[3]; // Use radio-defined option value - - #ifdef FAILSAFE_ENABLE - bool failsafe=false; - if(rx_ok_buff[0]&0x02) { // Packet contains failsafe instead of channels - failsafe=true; - rx_ok_buff[0]&=0xFD; //remove the failsafe flag - } - #endif - if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) ) - { // New model has been selected - CHANGE_PROTOCOL_FLAG_on; //change protocol - WAIT_BIND_off; - if((rx_ok_buff[1]&0x80)!=0 || IS_AUTOBIND_FLAG_on) - BIND_IN_PROGRESS; //launch bind right away if in autobind mode or bind is set - else - BIND_DONE; - protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0] - sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6 - RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3 - } else if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set - { // Restart protocol with bind - CHANGE_PROTOCOL_FLAG_on; - BIND_IN_PROGRESS; - } else { - if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset - { // Request protocol to end bind - #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO) - if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYV) - BIND_DONE; - else - #endif - if(bind_counter>2) - bind_counter=2; - } - } - - //store current protocol values - for(uint8_t i=0;i<3;i++) - cur_protocol[i] = rx_ok_buff[i]; - - // decode channel/failsafe values - volatile uint8_t *p=rx_ok_buff+3; - uint8_t dec=-3; - for(uint8_t i=0;i=8) { - dec-=8; - p++; - } - p++; - uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF; - if(failsafe) - Failsafe_data[i]=temp; //value range 0..2047, 0=no pulses, 2047=hold - else - Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125% - } - cli(); - if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid - { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer - RX_MISSED_BUFF_off; - } - sei(); } void modules_reset() diff --git a/remote/src/cc2500_spi.cpp b/remote/src/cc2500_spi.cpp index 35fa423..af0cedf 100644 --- a/remote/src/cc2500_spi.cpp +++ b/remote/src/cc2500_spi.cpp @@ -107,9 +107,9 @@ void CC2500_SetPower() uint8_t power=CC2500_BIND_POWER; if(IS_BIND_DONE) { #ifdef CC2500_ENABLE_LOW_POWER - power=IS_POWER_FLAG_on?CC2500_HIGH_POWER:CC2500_LOW_POWER; + power = IS_POWER_FLAG_on ? CC2500_HIGH_POWER : CC2500_LOW_POWER; #else - power=CC2500_HIGH_POWER; + power = CC2500_HIGH_POWER; #endif } if(IS_RANGE_FLAG_on) diff --git a/remote/src/crc.cpp b/remote/src/crc.cpp index 0efd817..133557b 100644 --- a/remote/src/crc.cpp +++ b/remote/src/crc.cpp @@ -1,22 +1,28 @@ -#include "Arduino.h" +#include "crc.h" -static uint32_t crc_table[16] = { +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 crc_update (uint32_t crc, uint8_t data) -{ - uint8_t tbl_idx; - uint8_t mask = 0x0f; - tbl_idx = crc ^ (data >> (0 * 4)); - crc = (crc_table + (tbl_idx & mask)) ^ (crc >> 4); +uint32_t __tiny_crc32(const void *data, unsigned int length, uint32_t crc) +{ + const unsigned char *buf = (const unsigned char *)data; + unsigned int i; - tbl_idx = crc ^ (data >> (1 * 4)); - crc = (crc_table + (tbl_idx & mask)) ^ (crc >> 4); + 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; + 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; } diff --git a/remote/src/eeprom.cpp b/remote/src/eeprom.cpp index 90ea84c..5464884 100644 --- a/remote/src/eeprom.cpp +++ b/remote/src/eeprom.cpp @@ -22,6 +22,8 @@ Eeprom_config::~Eeprom_config() { } int Eeprom_config::validate() { + uint32_t crc_calc = 0; + if (this->sucessfull_read == false) { return -1; } @@ -29,12 +31,8 @@ int Eeprom_config::validate() { if (this->current_config.version != CURRENT_VERSION) { return -1; } - uint32_t crc_calc = 0; - uint8_t * data = &(this->current_config.data); - for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i = 0) { - crc_calc = crc_update(crc_calc, data[i]); - } + crc_calc = tiny_crc32(&(this->current_config.data), sizeof(this->current_config.data)); if (crc_calc != this->current_config.data_crc) { return -1; @@ -47,8 +45,7 @@ int Eeprom_config::read(void) { data = (uint8_t *) &this->current_config; for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i = 0) { - EEPROM.read(0x10 + i , data); - data+=1; + data[i] = EEPROM.read(0x10 + i); } this->sucessfull_read = true; @@ -59,27 +56,29 @@ 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 = 0) { - EEPROM.write(0x10 + i , data); - data+=1; + EEPROM.write(0x10 + i , data[i]); } return 0; } -int Eeprom_config::get_ch_config(struct ch_config* config) { +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 ch_config) * CH_COUNT); + memcpy(config, &this->current_config.data.ch, sizeof(struct Input::ch_config) * Input::CH_COUNT); return 0; } -int Eeprom_config::set_ch_config(struct ch_config* config) { +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 ch_config) * CH_COUNT); + memcpy(this->current_config.data.ch, config, sizeof(struct Input::ch_config) * Input::CH_COUNT); return 0; } diff --git a/remote/src/input.cpp b/remote/src/input.cpp index 8ef16fa..06d9bff 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -30,8 +30,8 @@ Input::Input(void) { //InitFailsafe for (uint8_t i = 0; i < NUM_TX_CHN; i++) - Failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; - Failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% + this->failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; + this->failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% // init channel @@ -40,7 +40,7 @@ Input::Input(void) { this->channel_data[CH_THROTTLE] = 204; } -uint16_t *Input::get_channel_data(void) { +uint16_t* Input::get_channel_data(void) { return this->channel_data; } void Input::mark_processed(void) { @@ -49,14 +49,6 @@ void Input::mark_processed(void) { this->curr = temp; } -struct Input::data* Input::get_curr_input(void) { - return this->curr; -} - -struct Input::data* Input::get_old_input(void) { - return this->old; -} - void Input::init() { this->pins[CH_THROTTLE] = Throttle_pin; this->ch_config[CH_THROTTLE].is_analog = true; diff --git a/remote/src/state.cpp b/remote/src/state.cpp index 6f0491b..b9069a9 100644 --- a/remote/src/state.cpp +++ b/remote/src/state.cpp @@ -96,299 +96,3 @@ void update_state(void) { curr_state->enter(); } } - -//LCD_state_init - -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; - diff = millis() - this->time_enter; - if (diff > 5 * 1000) { - new_state = s_joy; - } -} -void LCD_state_init::leave(void) -{ - lcd.clear(); -} -//LCD_state_bind -LCD_state_bind::LCD_state_bind(void) { - this->bind_time = 20; -} -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) -{ - debugln("blubber\n"); - 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(); - while(1) { - start = end__; - next_callback_time = ReadFrSky_2way(); - - - // 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); - - uint32_t wait_until = start + next_callback_time; - 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(); -} - - -// -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 ch_config ch_config[CH_COUNT]; - input.get_calibration(ch_config); - - - 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"); -} - -// ######################################################### -// LCD_state_menu -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 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]); - - 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(); -} - -// LCD_state_fly -LCD_state_fly::LCD_state_fly(void) { -} - -void LCD_state_fly::enter(void) { - lcd.setCursor(0,0); - lcd.print("fly mode "); - lcd.setCursor(0,1); - lcd.print(" "); - this->time_enter = millis(); -} - -void LCD_state_fly::update(void) -{ - -} -void LCD_state_fly::leave(void) -{ - lcd.clear(); -} diff --git a/remote/src/state_bind.cpp b/remote/src/state_bind.cpp new file mode 100644 index 0000000..9f1f075 --- /dev/null +++ b/remote/src/state_bind.cpp @@ -0,0 +1,77 @@ +#include +#include +#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) +{ + debugln("blubber\n"); + 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(); + 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); + + uint32_t wait_until = start + next_callback_time; + 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(); +} diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp new file mode 100644 index 0000000..d61a92d --- /dev/null +++ b/remote/src/state_fly.cpp @@ -0,0 +1,29 @@ +#include +#include +#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_fly::LCD_state_fly(void) { +} + +void LCD_state_fly::enter(void) { + lcd.setCursor(0,0); + lcd.print("fly mode "); + lcd.setCursor(0,1); + lcd.print(" "); + this->time_enter = millis(); +} + +void LCD_state_fly::update(void) +{ + +} +void LCD_state_fly::leave(void) +{ + lcd.clear(); +} diff --git a/remote/src/state_init.cpp b/remote/src/state_init.cpp new file mode 100644 index 0000000..1c4885c --- /dev/null +++ b/remote/src/state_init.cpp @@ -0,0 +1,31 @@ +#include +#include +#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; + diff = millis() - this->time_enter; + if (diff > 5 * 1000) { + new_state = s_joy; + } +} +void LCD_state_init::leave(void) +{ + lcd.clear(); +} diff --git a/remote/src/state_joy_calib.cpp b/remote/src/state_joy_calib.cpp new file mode 100644 index 0000000..3283b1d --- /dev/null +++ b/remote/src/state_joy_calib.cpp @@ -0,0 +1,115 @@ +#include +#include +#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); + + + 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"); +} diff --git a/remote/src/state_menu.cpp b/remote/src/state_menu.cpp new file mode 100644 index 0000000..17ba407 --- /dev/null +++ b/remote/src/state_menu.cpp @@ -0,0 +1,84 @@ +#include +#include +#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 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]); + + 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(); +} From fdd5004e344926ec57dd70dfd9b20d69132c7c2f Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 00:57:12 +0200 Subject: [PATCH 04/10] fix eeprom and add fly state implementation --- remote/include/FrSkyD_cc2500.h | 1 + remote/include/state.h | 13 ++- remote/src/FrSkyD_cc2500.cpp | 19 +++-- remote/src/Multiprotocol.cpp | 39 --------- remote/src/eeprom.cpp | 16 +++- remote/src/input.cpp | 2 +- remote/src/state.cpp | 42 ---------- remote/src/state_bind.cpp | 18 +++-- remote/src/state_fly.cpp | 140 +++++++++++++++++++++++++++++++++ remote/src/state_init.cpp | 21 +++++ remote/src/state_joy_calib.cpp | 19 +++++ 11 files changed, 228 insertions(+), 102 deletions(-) diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h index 09417c3..f14888b 100644 --- a/remote/include/FrSkyD_cc2500.h +++ b/remote/include/FrSkyD_cc2500.h @@ -17,6 +17,7 @@ #define _FRSKYD_CC2500_H_ #include +extern uint16_t state; void Frsky_init_hop(void); void frsky2way_init(uint8_t bind); diff --git a/remote/include/state.h b/remote/include/state.h index 27371f6..8555f3f 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -5,8 +5,16 @@ #include #include - extern LiquidCrystal_I2C lcd; +enum lcd_special_chars { + battery_char = 0, + rssiantenna= 1, + rssi_bars = 2, + clock_char = 3, + MAX_SPECIAL_CHARS =8, +}; + + void init_state(void); void update_state(void); @@ -53,6 +61,9 @@ public: class LCD_state_fly: public State { private: unsigned long time_enter; + void print_akku(uint8_t akku_quad, uint8_t akku_remote); + void print_rssi(uint8_t rssi_percent); + void print_time(uint16_t time); public: LCD_state_fly(void); diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp index 73acc6c..fc55255 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -18,19 +18,21 @@ #include "Multiprotocol.h" #include "input.h" #include "FrSkyD_cc2500.h" +#include "common.h" uint8_t hopping_frequency[50]; +uint16_t state; uint16_t counter; void frsky2way_init(uint8_t bind) { - //debugln("frsky2way_init"); + //debugln("frsky2way_init"); FRSKY_init_cc2500(FRSKYD_cc2500_conf); CC2500_WriteReg(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]); CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x05); CC2500_Strobe(CC2500_SIDLE); // Go to idle... - // + CC2500_WriteReg(CC2500_0A_CHANNR, 0x00); CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); CC2500_Strobe(CC2500_SFRX); @@ -104,7 +106,6 @@ uint16_t initFrSky_2way() if(IS_BIND_IN_PROGRESS) { - frsky2way_init(1); state = FRSKY_BIND; debugln("initFrSky_2way bind"); } else { @@ -140,15 +141,16 @@ uint16_t ReadFrSky_2way_bind(void) uint16_t ReadFrSky_2way() { if (state <= FRSKY_BIND_DONE) { - //debugln("%s bind done",__func__); - state = FRSKY_DATA2; frsky2way_init(0); + //debugln("%s bind done",__func__); + + state = FRSKY_DATA2; counter = 0; } else if (state == FRSKY_DATA5) { - CC2500_Strobe(CC2500_SRX);//0x34 RX enable - state = FRSKY_DATA1; - return 9200; + CC2500_Strobe(CC2500_SRX);//0x34 RX enable + state = FRSKY_DATA1; + return 9200; } counter = (counter + 1) % 188; @@ -189,6 +191,7 @@ uint16_t ReadFrSky_2way() } CC2500_Strobe(CC2500_SIDLE); CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); + if ( prev_option != option ) { CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack prev_option = option ; diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index 797287d..e589792 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -75,7 +75,6 @@ uint8_t flags; uint16_t crc; uint8_t crc8; uint16_t failsafe_count; -uint16_t state; uint8_t len; #if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO) @@ -248,11 +247,9 @@ void loop() uint32_t s; s =micros(); input.update(); - debug("input took %lu", (micros()-s)); s =micros(); update_state(); - debugln("state took %lu", (micros()-s)); return; uint32_t next_callback; @@ -282,7 +279,6 @@ void loop() uint32_t wait = next_callback; wait -= ((end__-start)); delayMicroseconds(wait); - end__ += wait; } end__ = micros(); } @@ -328,39 +324,4 @@ void set_rx_tx_addr(uint32_t id) uint32_t random_id(bool create_new) { - #ifdef FORCE_GLOBAL_ID - (void)create_new; - return FORCE_GLOBAL_ID; - #else - uint32_t id=0; - - if (eeprom_read_byte((EE_ADDR)(10))==0xf0 && !create_new) { - // TXID exists in EEPROM - for(uint8_t i=4;i>0;i--) { - id<<=8; - id|=eeprom_read_byte((EE_ADDR)i-1); - } - if(id!=0x2AD141A7) //ID with seed=0 - { - debugln("Read ID from EEPROM"); - return id; - } - } - // Generate a random ID from UUID - #define STM32_UUID ((uint32_t *)0x1FFFF7E8) - if (!create_new) { - id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; - debugln("Generated ID from STM32 UUID %x", id); - } else { - id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16); - } - - #if 0 - for(uint8_t i=0;i<4;i++) - eeprom_write_byte((EE_ADDR)address+i,id >> (i*8)); - eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom. - #endif - return id; - #endif - } diff --git a/remote/src/eeprom.cpp b/remote/src/eeprom.cpp index 5464884..a117468 100644 --- a/remote/src/eeprom.cpp +++ b/remote/src/eeprom.cpp @@ -2,6 +2,8 @@ #include #include +#include + #include "tx_def.h" #include "eeprom.h" #include "crc.h" @@ -25,29 +27,37 @@ 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 %d vs %d \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 version %d vs %d \n", crc_calc, this->current_config.data_crc); return -1; } - return 0; + debugln("valid config\n"); + return 1; } int Eeprom_config::read(void) { uint8_t *data = NULL; + debugln("enter \n"); data = (uint8_t *) &this->current_config; - for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i = 0) { + debugln("for start\n"); + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { data[i] = EEPROM.read(0x10 + i); + debugln("Read %d\n",i); } + debugln("Read end\n"); this->sucessfull_read = true; return 0; } @@ -59,7 +69,7 @@ int Eeprom_config::write(void) { 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 = 0) { + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { EEPROM.write(0x10 + i , data[i]); } diff --git a/remote/src/input.cpp b/remote/src/input.cpp index 06d9bff..dfdab8c 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -138,7 +138,7 @@ bool Input::is_low(enum Input::input_channels ch) { } bool Input::is_menu_triggered(void) { - return this->curr->menu; + return this->curr->menu != this->old->menu; } void Input::invert_ch(enum Input::input_channels ch) { diff --git a/remote/src/state.cpp b/remote/src/state.cpp index b9069a9..bd3b669 100644 --- a/remote/src/state.cpp +++ b/remote/src/state.cpp @@ -20,47 +20,6 @@ State *s_joy = NULL; State *s_menu = NULL; -enum lcd_special_chars { - battery_66 = 0, - battery_33 = 1, - battery_0 = 2, - battery_100 = 3, - rssiantenna = 4, - rssi_bars_1 = 5, - rssi_bars_2 = 6, - rssi_bars_3 = 7, - MAX_SPECIAL_CHARS =8, -}; -// 6 Byte-Arrays für 6 verschiedene Batteriesymbole -__extension__ struct lcd_special_chars_data { byte data[MAX_SPECIAL_CHARS]; } - lcd_special_chars_data[MAX_SPECIAL_CHARS] = -{ - //[battery_0] = - { 0b01110, 0b11011, 0b10001, 0b10001, 0b10001, 0b10001, 0b10001, 0b11111 }, - //[battery_33] = - { 0b01110, 0b11011, 0b10001, 0b10001, 0b10001, 0b11111, 0b11111, 0b11111 }, - //[battery_66] = - { 0b01110, 0b11011, 0b10001, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 }, - //[battery_100] = - { 0b01110, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 }, - //[rssiantenna] = - { 0b10101, 0b10101, 0b01110, 0b00100, 0b00100, 0b00101, 0b00101, 0b00101 }, - //[rssi_bars_1] = - { 0b00001, 0b00001, 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101 }, - //[rssi_bars_2] = - { 0b00001, 0b00001, 0b00001, 0b00001, 0b10101, 0b10101, 0b10101, 0b10101 }, - //[rssi_bars_3] = - { 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101, 0b10101, 0b10101 }, -}; - - -void install_special_caracters(void) -{ - for(int i = 0; i < MAX_SPECIAL_CHARS; i ++) { - lcd.createChar(i, lcd_special_chars_data[i].data); - } -} - void init_state(void) { #if 0 Wire.setSDA(PB9); @@ -78,7 +37,6 @@ void init_state(void) { curr_state = NULL; new_state = s_init; - install_special_caracters(); update_state(); } diff --git a/remote/src/state_bind.cpp b/remote/src/state_bind.cpp index 9f1f075..8561766 100644 --- a/remote/src/state_bind.cpp +++ b/remote/src/state_bind.cpp @@ -23,20 +23,22 @@ void LCD_state_bind::enter(void) { void LCD_state_bind::update(void) { - debugln("blubber\n"); - 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; + 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); + 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(); diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp index d61a92d..e61baa0 100644 --- a/remote/src/state_fly.cpp +++ b/remote/src/state_fly.cpp @@ -7,6 +7,7 @@ #include "eeprom.h" #include "debug.h" #include "tx_def.h" +#include "common.h" LCD_state_fly::LCD_state_fly(void) { } @@ -19,8 +20,147 @@ void LCD_state_fly::enter(void) { this->time_enter = millis(); } +void LCD_state_fly::print_time(uint16_t time) +{ + /** + * 0123456789012345 + * fly mode A PP + * T SSS AA PP A PP + **/ + + char line[17]; + lcd.setCursor(0,1); + byte clock_char_data[8] = { + 0b01110, + 0b10101, + 0b10101, + 0b10111, + 0b10001, + 0b10001, + 0b01110, + 0b00000 + }; + lcd.createChar(clock_char, clock_char_data); + lcd.write(clock_char); + + snprintf(line, sizeof(line), "%*d", 3, time); + lcd.print(line); +} +void LCD_state_fly::print_akku(uint8_t akku_quad, 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 (uint8_t i = 0; i < 2; ++i) { + lcd.setCursor(12,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, battery_char_data); + lcd.write(battery_char); + snprintf(line, sizeof(line), " %*d", 2, akku[i]); + lcd.print(line); + } +} +void LCD_state_fly::print_rssi(uint8_t rssi_percent) +{ + char line[17]; + byte rssi_antenna[8] = { 0b10101, 0b10101, 0b01110, 0b00100, 0b00100, 0b00101, 0b00101, 0b00101 }; + + byte rssi_antenna_1[8] = { 0b00001, 0b00001, 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101 }; + byte rssi_antenna_2[8] = { 0b00001, 0b00001, 0b00001, 0b00001, 0b10101, 0b10101, 0b10101, 0b10101 }; + byte rssi_antenna_3[8] = { 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101, 0b10101, 0b10101 }; + + lcd.setCursor(6,1); + lcd.createChar(rssiantenna, rssi_antenna); + + lcd.write(rssiantenna); + if (rssi_percent > 75) {/* 100-75 -> 3 */ + lcd.createChar(rssi_bars, rssi_antenna_3); + lcd.write(rssi_bars); + } else if (rssi_percent > 50) { /* 74-50 -> 2 */ + lcd.createChar(rssi_bars, rssi_antenna_2); + lcd.write(rssi_bars); + } else if (rssi_percent > 25) { /* 49-25 -> 1 */ + lcd.createChar(rssi_bars, rssi_antenna_1); + lcd.write(rssi_bars); + } else { /* 25-0 -> 0 */ + lcd.write(' '); + } + + snprintf(line, sizeof(line), " %*d", 2, rssi_percent); + lcd.print(line); +} + 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 + + snprintf(line, sizeof(line), "fly mode %02lu sec", time_in_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(); + + if (state == FRSKY_DATA1) { + // update time + time_in_ms = millis() - this->time_enter; + time_in_s = time_in_ms/1000; // to sec + + // print on lcd + this->print_time(time_in_s); + + rssi_percent += 1; + this->print_time(rssi_percent); + + akku_quad += 1; + akku_remote += 2; + this->print_akku(akku_quad, akku_remote); + + input.update(); + } + + + end__ = micros(); + if (end__ - start < next_callback_time) { + uint32_t wait = next_callback_time; + wait -= (end__ - start); + delayMicroseconds(wait); + } + end__ = micros(); + + if (input.is_menu_triggered() == 0) + break; + + } + + // cange to menu when done + new_state = s_menu; } void LCD_state_fly::leave(void) diff --git a/remote/src/state_init.cpp b/remote/src/state_init.cpp index 1c4885c..c7db9d9 100644 --- a/remote/src/state_init.cpp +++ b/remote/src/state_init.cpp @@ -20,9 +20,30 @@ void LCD_state_init::enter(void) { void LCD_state_init::update(void) { uint32_t diff; + delay(100); diff = millis() - this->time_enter; if (diff > 5 * 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); } } void LCD_state_init::leave(void) diff --git a/remote/src/state_joy_calib.cpp b/remote/src/state_joy_calib.cpp index 3283b1d..bfc959e 100644 --- a/remote/src/state_joy_calib.cpp +++ b/remote/src/state_joy_calib.cpp @@ -96,6 +96,25 @@ void LCD_state_joy_calibration::update(void) { 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 %x", 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); From 85b0024366490d70acb6e5f42f0bfaec603193bd Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 12:45:51 +0200 Subject: [PATCH 05/10] implement fly mode with lcd --- remote/include/FrSkyD_cc2500.h | 20 +++ remote/include/Multiprotocol.h | 10 -- remote/include/common.h | 51 ------ remote/include/config.h | 1 - remote/include/eeprom.h | 2 +- remote/include/input.h | 10 +- remote/include/pins.h | 31 ++-- remote/include/state.h | 11 +- remote/src/FrSkyD_cc2500.cpp | 113 +++++++++++- remote/src/Multiprotocol.cpp | 246 +++------------------------ remote/src/{common.cpp => common.ds} | 19 --- remote/src/config.cpp | 3 +- remote/src/eeprom.cpp | 8 +- remote/src/input.cpp | 44 +++-- remote/src/state_bind.cpp | 1 - remote/src/state_fly.cpp | 173 ++++++++++++------- remote/src/state_init.cpp | 6 + remote/src/state_menu.cpp | 1 + 18 files changed, 327 insertions(+), 423 deletions(-) delete mode 100644 remote/include/common.h rename remote/src/{common.cpp => common.ds} (88%) diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h index f14888b..661b8eb 100644 --- a/remote/include/FrSkyD_cc2500.h +++ b/remote/include/FrSkyD_cc2500.h @@ -17,8 +17,27 @@ #define _FRSKYD_CC2500_H_ #include + +enum { + FRSKY_BIND = 0, + FRSKY_BIND_DONE = 1000, + FRSKY_DATA1, + FRSKY_DATA2, + FRSKY_DATA3, + FRSKY_DATA4, + FRSKY_DATA5 +}; + +#define MAX_PKT 29 +extern uint8_t pkt[MAX_PKT];//telemetry receiving packets + + +extern uint8_t freq_offset; extern uint16_t state; + void Frsky_init_hop(void); +void FRSKY_init_cc2500(const uint8_t *ptr); + void frsky2way_init(uint8_t bind); void frsky2way_build_bind_packet(); @@ -30,4 +49,5 @@ uint16_t ReadFrSky_2way(void); uint16_t ReadFrSky_2way_bind(void); uint16_t convert_channel_frsky(uint8_t num); +void set_rx_tx_addr(uint32_t id); #endif diff --git a/remote/include/Multiprotocol.h b/remote/include/Multiprotocol.h index a917028..8fc35d4 100644 --- a/remote/include/Multiprotocol.h +++ b/remote/include/Multiprotocol.h @@ -24,24 +24,15 @@ #include "stdint.h" void set_rx_tx_addr(uint32_t id); -#define MAX_PKT 29 -extern uint8_t hopping_frequency_no; -extern uint8_t sub_protocol; -extern uint8_t calData[48]; -extern uint32_t MProtocol_id_master; extern uint8_t protocol_flags; extern uint8_t protocol_flags2; -extern uint8_t pkt[MAX_PKT];//telemetry receiving packets -extern uint8_t prev_option; extern uint8_t crc8; extern uint8_t packet_count; -extern uint8_t RX_num; 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 uint16_t seed; extern uint8_t phase; extern uint8_t len; @@ -86,7 +77,6 @@ enum MultiPacketTypes // #define BIND_BUTTON_FLAG_on protocol_flags |= _BV(5) #define BIND_BUTTON_FLAG_off protocol_flags &= ~_BV(5) -#define IS_BIND_BUTTON_FLAG_on ( ( protocol_flags & _BV(5) ) !=0 ) //Bind flag #define BIND_IN_PROGRESS protocol_flags &= ~_BV(7) #define BIND_DONE protocol_flags |= _BV(7) diff --git a/remote/include/common.h b/remote/include/common.h deleted file mode 100644 index 76b9a0b..0000000 --- a/remote/include/common.h +++ /dev/null @@ -1,51 +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 . -*/ -#ifndef _COMMON_H_ -#define _COMMON_H_ -#include -#include "config.h" -#include "tx_def.h" - -void InitChannel(void); - -/******************************/ -/** FrSky D and X routines **/ -/******************************/ -enum { - FRSKY_BIND = 0, - FRSKY_BIND_DONE = 1000, - FRSKY_DATA1, - FRSKY_DATA2, - FRSKY_DATA3, - FRSKY_DATA4, - FRSKY_DATA5 -}; - -/******************************/ -/** 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 diff --git a/remote/include/config.h b/remote/include/config.h index 59c8e24..310b467 100644 --- a/remote/include/config.h +++ b/remote/include/config.h @@ -163,7 +163,6 @@ /*** PPM MODE SETTINGS ***/ /*************************/ #define NBR_BANKS 1 -extern uint8_t curr_bank; struct PPM_Parameters { diff --git a/remote/include/eeprom.h b/remote/include/eeprom.h index 87e466f..3c65cc7 100644 --- a/remote/include/eeprom.h +++ b/remote/include/eeprom.h @@ -21,7 +21,7 @@ public: int set_master_id(uint32_t master_id); private: - #define CURRENT_VERSION 0x01 + #define CURRENT_VERSION 0x02 struct eeprom_data_v1 { uint8_t version; uint32_t data_crc; diff --git a/remote/include/input.h b/remote/include/input.h index ea5e995..95c6db9 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -18,7 +18,6 @@ class Input { CH_AUX3 = 6, CH_AUX4 = 7, CH_AUX5 = 8, - CH_AUX6 = 8, CH_MAX = 8, CH_COUNT = 9, @@ -49,6 +48,7 @@ class Input { void invert_ch(enum input_channels ch); void print_ch(enum input_channels ch); + void print(); void calibration_reset(void); bool calibration_update(void); @@ -69,8 +69,8 @@ class Input { }; // actual tx channel data - uint16_t channel_data[CH_COUNT]; - uint16_t failsafe_data[CH_COUNT]; + uint16_t channel_data[NUM_TX_CHN]; + uint16_t failsafe_data[NUM_TX_CHN]; struct data input[2]; struct data* curr; @@ -80,9 +80,9 @@ class Input { struct ch_config ch_config[CH_COUNT]; // pin setttings - uint32_t pins[CH_COUNT]; + int pins[CH_COUNT]; }; -extern const char* ch_name[NUM_TX_CHN]; +extern const char* ch_name[Input::CH_COUNT]; extern Input input; #endif diff --git a/remote/include/pins.h b/remote/include/pins.h index f8c1321..affd018 100644 --- a/remote/include/pins.h +++ b/remote/include/pins.h @@ -18,12 +18,6 @@ #include "Arduino.h" -#define LED_off -#define LED_on -#define LED_output -#define IS_LED_on false -#define LED_toggle - // #define SDI_pin PA0 /* SDIO MOSI */ #define SCLK_pin PA1 /* SCLK */ @@ -37,18 +31,29 @@ #define Roll_pin PA7 #define Pitch_pin PA6 -#define Aux1_pin PB0 -#define Aux2_pin PB0 -#define Aux3_pin PB0 -#define Aux4_pin PB0 -#define Aux5_pin PB0 -#define Aux6_pin PB0 -#define Menu_pin PB0 +#define Aux1_pin PB1 +#define Aux2_pin PB10 +#define Aux3_pin PB11 +#define Aux4_pin PB13 +#define Aux5_pin PB15 +#define Menu_pin PB12 + +#define Battery_pin PA9 +#define Led_pin PC13 +#define Buzzer_pin PA8 #define cli() #define sei() #define NOP() __asm__ __volatile__("nop") +#define BUZZER_off digitalWrite(Buzzer_pin, LOW) +#define BUZZER_on digitalWrite(Buzzer_pin, HIGH) +#define BUZZER_output pinMode(Buzzer_pin, OUTPUT) + +#define LED_off digitalWrite(Led_pin, LOW) +#define LED_on digitalWrite(Led_pin, HIGH) +#define LED_output pinMode(Led_pin,OUTPUT) + #define SDI_on digitalWrite(SDI_pin, HIGH) #define SDI_off digitalWrite(SDI_pin, LOW) #define SDI_1 (digitalRead(SDI_pin) == HIGH) diff --git a/remote/include/state.h b/remote/include/state.h index 8555f3f..b8464bd 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -8,9 +8,12 @@ extern LiquidCrystal_I2C lcd; enum lcd_special_chars { battery_char = 0, - rssiantenna= 1, - rssi_bars = 2, - clock_char = 3, + battery_char_1 = 0, + battery_char_2 = 1, + + rssiantenna= 3, + rssi_bars = 4, + clock_char = 5, MAX_SPECIAL_CHARS =8, }; @@ -61,6 +64,8 @@ public: class LCD_state_fly: public State { private: unsigned long time_enter; + uint16_t last_time; + void print_akku(uint8_t akku_quad, uint8_t akku_remote); void print_rssi(uint8_t rssi_percent); void print_time(uint16_t time); diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp index fc55255..1d94648 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -13,17 +13,85 @@ along with Multiprotocol. If not, see . */ -#include "common.h" #include "cc2500_spi.h" #include "Multiprotocol.h" #include "input.h" #include "FrSkyD_cc2500.h" -#include "common.h" +uint8_t rx_tx_addr[5]; uint8_t hopping_frequency[50]; +uint8_t pkt[MAX_PKT];//telemetry receiving packets + uint16_t state; uint16_t counter; +uint8_t freq_offset; +uint8_t freq_offset_prev; + +const uint8_t FRSKY_common_startreg_cc2500_conf[] = { + CC2500_02_IOCFG0 , + CC2500_00_IOCFG2 , + CC2500_17_MCSM1 , + CC2500_18_MCSM0 , + CC2500_06_PKTLEN , + CC2500_07_PKTCTRL1 , + CC2500_08_PKTCTRL0 , + CC2500_3E_PATABLE , + CC2500_0B_FSCTRL1 , + CC2500_0C_FSCTRL0 , // replaced by frequency offset value + CC2500_0D_FREQ2 , + CC2500_0E_FREQ1 , + CC2500_0F_FREQ0 , + CC2500_10_MDMCFG4 , + CC2500_11_MDMCFG3 , + CC2500_12_MDMCFG2 , + CC2500_13_MDMCFG1 , + CC2500_14_MDMCFG0 , + CC2500_15_DEVIATN +}; + +const uint8_t FRSKY_common_end_cc2500_conf[][2] = { + { CC2500_19_FOCCFG, 0x16 }, + { CC2500_1A_BSCFG, 0x6c }, + { CC2500_1B_AGCCTRL2, 0x43 }, + { CC2500_1C_AGCCTRL1, 0x40 }, + { CC2500_1D_AGCCTRL0, 0x91 }, + { CC2500_21_FREND1, 0x56 }, + { CC2500_22_FREND0, 0x10 }, + { CC2500_23_FSCAL3, 0xa9 }, + { CC2500_24_FSCAL2, 0x0A }, + { CC2500_25_FSCAL1, 0x00 }, + { CC2500_26_FSCAL0, 0x11 }, + { CC2500_29_FSTEST, 0x59 }, + { CC2500_2C_TEST2, 0x88 }, + { CC2500_2D_TEST1, 0x31 }, + { CC2500_2E_TEST0, 0x0B }, + { CC2500_03_FIFOTHR, 0x07 }, + { CC2500_09_ADDR, 0x00 } +}; + +const uint8_t FRSKYD_cc2500_conf[] = { + /*02_IOCFG0*/ 0x06 , + /*00_IOCFG2*/ 0x06 , + /*17_MCSM1*/ 0x0c , + /*18_MCSM0*/ 0x18 , + /*06_PKTLEN*/ 0x19 , + /*07_PKTCTRL1*/ 0x04 , + /*08_PKTCTRL0*/ 0x05 , + /*3E_PATABLE*/ 0xff , + /*0B_FSCTRL1*/ 0x08 , + /*0C_FSCTRL0*/ 0x00 , + /*0D_FREQ2*/ 0x5c , + /*0E_FREQ1*/ 0x76 , + /*0F_FREQ0*/ 0x27 , + /*10_MDMCFG4*/ 0xAA , + /*11_MDMCFG3*/ 0x39 , + /*12_MDMCFG2*/ 0x11 , + /*13_MDMCFG1*/ 0x23 , + /*14_MDMCFG0*/ 0x7a , + /*15_DEVIATN*/ 0x42 +}; + void frsky2way_init(uint8_t bind) { //debugln("frsky2way_init"); @@ -39,7 +107,7 @@ void frsky2way_init(uint8_t bind) //#######END INIT######## } -void frsky2way_build_bind_packet() +void frsky2way_build_bind_packet(void) { //debugln("build bind"); //11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01 @@ -192,10 +260,12 @@ uint16_t ReadFrSky_2way() CC2500_Strobe(CC2500_SIDLE); CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]); - if ( prev_option != option ) { - CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack - prev_option = option ; + // Frequency offset update + if ( freq_offset_prev != freq_offset) { + CC2500_WriteReg(CC2500_0C_FSCTRL0, freq_offset); + freq_offset_prev = freq_offset; } + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); CC2500_Strobe(CC2500_SFRX); frsky2way_data_frame(); @@ -236,3 +306,34 @@ void Frsky_init_hop(void) hopping_frequency[i] = i > 46 ? 0 : val; } } + +void FRSKY_init_cc2500(const uint8_t *ptr) +{ + for (uint8_t i = 0; i < 19; i++) + { + uint8_t reg = FRSKY_common_startreg_cc2500_conf[i]; + uint8_t val = ptr[i]; + if (reg == CC2500_0C_FSCTRL0) + val = freq_offset; + CC2500_WriteReg(reg, val); + } + freq_offset_prev = freq_offset; + for (uint8_t i = 0; i < 17; i++) + { + uint8_t reg = FRSKY_common_end_cc2500_conf[i][0]; + uint8_t val = FRSKY_common_end_cc2500_conf[i][1]; + CC2500_WriteReg(reg, val); + } + CC2500_SetTxRxMode(TX_EN); + CC2500_SetPower(); + CC2500_Strobe(CC2500_SIDLE); // Go to idle... +} + +void set_rx_tx_addr(uint32_t id) +{ // Used by almost all protocols + rx_tx_addr[0] = (id >> 24) & 0xFF; + rx_tx_addr[1] = (id >> 16) & 0xFF; + rx_tx_addr[2] = (id >> 8) & 0xFF; + rx_tx_addr[3] = (id >> 0) & 0xFF; + rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F); +} diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index e589792..a2c0f45 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -33,7 +33,6 @@ #include "pins.h" #include "Validate.h" -#include "common.h" #include "state.h" #include "input.h" #include "cc2500_spi.h" @@ -43,21 +42,14 @@ #include "FrSkyD_cc2500.h" //Global constants/variables -uint32_t MProtocol_id;//tx id, -uint32_t MProtocol_id_master; uint32_t blink=0,last_signal=0; uint8_t protocol_flags=0,protocol_flags2=0; // uint8_t channel; uint8_t packet[40]; -static void protocol_init(void); - -uint16_t seed; - // Protocol variables uint8_t cyrfmfg_id[6];//for dsm2 and devo -uint8_t rx_tx_addr[5]; uint8_t rx_id[4]; uint8_t phase; uint16_t bind_counter; @@ -68,260 +60,66 @@ uint8_t packet_count; uint8_t packet_sent; uint8_t packet_length; uint8_t *hopping_frequency_ptr; -uint8_t hopping_frequency_no=0; uint8_t rf_ch_num; -uint8_t throttle, rudder, elevator, aileron; -uint8_t flags; uint16_t crc; uint8_t crc8; uint16_t failsafe_count; uint8_t len; -#if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO) - uint8_t calData[48]; -#endif - - -// Mode_select variables -uint8_t mode_select; - -#if not defined (ORANGE_TX) && not defined (STM32_BOARD) -//Random variable -volatile uint32_t gWDT_entropy=0; -#endif - -//Serial protocol -uint8_t sub_protocol; -uint8_t protocol; -uint8_t option; -uint8_t cur_protocol[3]; -uint8_t prev_option; -uint8_t RX_num; - -//Serial RX variables -#define BAUD 100000 -#define RXBUFFER_SIZE 26 -volatile uint8_t rx_buff[RXBUFFER_SIZE]; -volatile uint8_t rx_ok_buff[RXBUFFER_SIZE]; -volatile uint8_t discard_frame = 0; - // Telemetry -uint8_t pkt[MAX_PKT];//telemetry receiving packets -float TIMER_PRESCALE = 5.82; -// Callback -typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer -void_function_t remote_callback = 0; - -//forward declarations -void modules_reset(); -uint32_t random_id(bool create_new); - -// Init void setup() { // Setup diagnostic uart before anything else #ifdef ENABLE_DBEUG Serial.begin(115200,SERIAL_8N1); + delay(1000); while (!Serial); // Wait for ever for the serial port to connect... + delay(1000); debugln("Multiprotocol startup"); debugln("time %s ", __TIME__); #endif - // all inputs - // outputs - SDI_output; - SCLK_output; - CC25_CSN_output; + // all inputs + // outputs + SDI_output; + SCLK_output; + CC25_CSN_output; - // Random - //random_init(); - CC25_CSN_on; - SDI_on; - SCLK_off; + // Random + //random_init(); + CC25_CSN_on; + SDI_on; + SCLK_off; //Wait for every component to start delay(100); // Read status of bind button - if( /*IS_BIND_BUTTON_on */ true) - { - BIND_BUTTON_FLAG_on; // If bind button pressed save the status - BIND_IN_PROGRESS; // Request bind - } - else - BIND_DONE; - - // Read status of mode select binary switch - // after this mode_select will be one of {0000, 0001, ..., 1111} - - - String str; - debugln("select bank:"); - str=""; - while(true) { - if (Serial.available() > 0) { - char recieved = Serial.read(); - if (recieved == '\n') { - int new_bank = atoi(str.c_str()); - curr_bank = new_bank; - debugln("Bank selection %d", new_bank); - break; - }else { - str += recieved; - } - } - } - - debugln("select mode:"); - str=""; - while(true) { - if (Serial.available() > 0) { - char recieved = Serial.read(); - if (recieved == '\n') { - int new_mode = atoi(str.c_str()); - debugln("Protocol selection switch reads as %d with \'%s\'", new_mode,str.c_str()); - mode_select = new_mode; - break; - }else { - str += recieved; - } - } - } - debugln("Protocol selection switch reads as %d", mode_select); + BIND_IN_PROGRESS; // Request bind // Update LED LED_off; LED_output; - //Init RF modules - modules_reset(); - - { - seed = analogRead(PA0); - randomSeed(seed); - } - - // Read or create protocol id - MProtocol_id_master=random_id(false); - - debugln("Module Id: %lx", MProtocol_id_master); - - //Protocol and interrupts initialization + //frquency offset initialization { - uint8_t line=curr_bank*14+mode_select-1; - protocol = PPM_prot[line].protocol; - cur_protocol[1] = protocol; - sub_protocol = PPM_prot[line].sub_proto; - RX_num = PPM_prot[line].rx_num; - - debug("protocol: %d ", protocol); - switch(protocol) { - case PROTO_FRSKYD: - debugln("PROTO_FRSKYD"); - break; - case PROTO_FRSKYX: - debugln("PROTO_FRSKYX"); - break; - case PROTO_FRSKYV: - debugln("PROTO_FRSKYV"); - break; - } - debug("sub_protocol: %d\n", sub_protocol); - option = PPM_prot[line].option; // Use radio-defined option value - debug("freq offset: %d\n", option); - line++; + freq_offset = 0; + debug("freq offset: %d\n", freq_offset); + CC2500_Reset(); - protocol_init(); + //Wait for cc2500 to reset + delay(100); } - debug("Init complete\n"); input.init(); input.update(); + init_state(); + + debug("Init complete\n"); } -// Main -// Protocol scheduler void loop() { - uint32_t s; - s =micros(); - input.update(); - - s =micros(); update_state(); - return; - uint32_t next_callback; - - uint32_t end__ = micros(); - uint32_t start = micros(); - - while(1) { - start = end__; - next_callback = remote_callback(); - - if (next_callback > 4000) { - uint32_t s; - - s =micros(); - input.update(); - debug("input took %lu", (micros()-s)); - - s =micros(); - update_state(); - debugln("state took %lu", (micros()-s)); - } - - uint32_t wait_until = start + next_callback; - end__ = micros(); - - if (end__-start < next_callback) { - uint32_t wait = next_callback; - wait -= ((end__-start)); - delayMicroseconds(wait); - } - end__ = micros(); - } -} - -// Protocol start -static void protocol_init() { - modules_reset(); // Reset all modules - uint32_t next_callback = 0; - - //Set global ID and rx_tx_addr - MProtocol_id = RX_num + MProtocol_id_master; - set_rx_tx_addr(MProtocol_id); - debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option); - - switch(protocol) // Init the requested protocol - { - case PROTO_FRSKYD: - next_callback = initFrSky_2way(); - remote_callback = ReadFrSky_2way; - break; - } - delayMicroseconds(next_callback); -} - -void modules_reset() -{ - CC2500_Reset(); - - //Wait for every component to reset - delay(100); -} - -// Convert 32b id to rx_tx_addr -void set_rx_tx_addr(uint32_t id) -{ // Used by almost all protocols - rx_tx_addr[0] = (id >> 24) & 0xFF; - rx_tx_addr[1] = (id >> 16) & 0xFF; - rx_tx_addr[2] = (id >> 8) & 0xFF; - rx_tx_addr[3] = (id >> 0) & 0xFF; - rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F); -} - -uint32_t random_id(bool create_new) -{ } diff --git a/remote/src/common.cpp b/remote/src/common.ds similarity index 88% rename from remote/src/common.cpp rename to remote/src/common.ds index 0235c52..1b3dc60 100644 --- a/remote/src/common.cpp +++ b/remote/src/common.ds @@ -147,25 +147,6 @@ uint8_t FRSKYXEU_cc2500_conf[] = { }; #endif -uint8_t FRSKY_common_end_cc2500_conf[][2] = { - { CC2500_19_FOCCFG, 0x16 }, - { CC2500_1A_BSCFG, 0x6c }, - { CC2500_1B_AGCCTRL2, 0x43 }, - { CC2500_1C_AGCCTRL1, 0x40 }, - { CC2500_1D_AGCCTRL0, 0x91 }, - { CC2500_21_FREND1, 0x56 }, - { CC2500_22_FREND0, 0x10 }, - { CC2500_23_FSCAL3, 0xa9 }, - { CC2500_24_FSCAL2, 0x0A }, - { CC2500_25_FSCAL1, 0x00 }, - { CC2500_26_FSCAL0, 0x11 }, - { CC2500_29_FSTEST, 0x59 }, - { CC2500_2C_TEST2, 0x88 }, - { CC2500_2D_TEST1, 0x31 }, - { CC2500_2E_TEST0, 0x0B }, - { CC2500_03_FIFOTHR, 0x07 }, - { CC2500_09_ADDR, 0x00 } -}; void FRSKY_init_cc2500(const uint8_t *ptr) { diff --git a/remote/src/config.cpp b/remote/src/config.cpp index 8beaf14..3a1f262 100644 --- a/remote/src/config.cpp +++ b/remote/src/config.cpp @@ -13,10 +13,9 @@ along with Multiprotocol. If not, see . */ #include "config.h" -uint8_t curr_bank = 0; const PPM_Parameters PPM_prot[14]= { //****************************** BANK 1 ****************************** -// Switch Protocol Sub protocol RX_Num Power Auto Bind Option +// Switch Protocol Sub protocol RX_Num Power Auto Bind Option /* 1 */ {PROTO_FRSKYD, 0 , 0 , P_LOW , AUTOBIND , 0 }, /* 2 */ {PROTO_FRSKYD, 0 , 0 , P_LOW , AUTOBIND , 20 }, /* 3 */ {PROTO_FRSKYD, 0 , 0 , P_LOW , AUTOBIND , 40 }, diff --git a/remote/src/eeprom.cpp b/remote/src/eeprom.cpp index a117468..14bae0f 100644 --- a/remote/src/eeprom.cpp +++ b/remote/src/eeprom.cpp @@ -32,14 +32,14 @@ int Eeprom_config::validate() { } if (this->current_config.version != CURRENT_VERSION) { - debugln("wrong version %d vs %d \n", 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 version %d vs %d \n", 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"); @@ -48,16 +48,12 @@ int Eeprom_config::validate() { int Eeprom_config::read(void) { uint8_t *data = NULL; - debugln("enter \n"); data = (uint8_t *) &this->current_config; - debugln("for start\n"); for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { data[i] = EEPROM.read(0x10 + i); - debugln("Read %d\n",i); } - debugln("Read end\n"); this->sucessfull_read = true; return 0; } diff --git a/remote/src/input.cpp b/remote/src/input.cpp index dfdab8c..e483276 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -9,7 +9,7 @@ Input input; -const char* ch_name[NUM_TX_CHN] = { +const char* ch_name[Input::CH_COUNT] = { "CH_ROLL", "CH_PITCH", "CH_THROTTLE", @@ -20,24 +20,23 @@ const char* ch_name[NUM_TX_CHN] = { "CH_AUX3", "CH_AUX4", "CH_AUX5", - "CH_AUX6" }; Input::Input(void) { + uint8_t i; this->curr = &(this->input[0]); this->old = &(this->input[1]); memset(this->input,0, sizeof(this->input)); - //InitFailsafe - for (uint8_t i = 0; i < NUM_TX_CHN; i++) + for (i = 0; i < NUM_TX_CHN; i++) { + //InitFailsafe this->failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; - this->failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% - - // init channel - - for (uint8_t i = 0; i < NUM_TX_CHN; i++) + // init channel this->channel_data[i] = 1024; - this->channel_data[CH_THROTTLE] = 204; + } + + this->failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% + this->channel_data[CH_THROTTLE] = CHANNEL_MIN_100; } uint16_t* Input::get_channel_data(void) { @@ -77,9 +76,6 @@ void Input::init() { this->pins[CH_AUX5] = Aux5_pin; this->ch_config[CH_AUX5].is_analog = false; - this->pins[CH_AUX6] = Aux6_pin; - this->ch_config[CH_AUX6].is_analog = false; - for (uint8_t i = 0; i < CH_COUNT; ++i) { pinMode(this->pins[i], INPUT); } @@ -96,7 +92,6 @@ void Input::init() { this->ch_config[CH_AUX2].inverted = false; this->ch_config[CH_AUX3].inverted = false; this->ch_config[CH_AUX4].inverted = false; - this->ch_config[CH_AUX5].inverted = false; } bool Input::is_centered(void) { @@ -145,10 +140,11 @@ void Input::invert_ch(enum Input::input_channels ch) { this->ch_config[ch].inverted = !this->ch_config[ch].inverted; } void Input::print_ch(enum Input::input_channels ch) { - debug("ch%d: %04d %04d min %d max %d high %d mid %d low %d\n", + debug("ch%d: %04d %04d %04d min %d max %d high %d mid %d low %d\n", ch, this->ch_raw[ch], this->curr->ch_data[ch], + this->old->ch_data[ch], this->ch_config[ch].min, this->ch_config[ch].max, this->is_high((enum Input::input_channels)ch), @@ -156,6 +152,17 @@ void Input::print_ch(enum Input::input_channels ch) { this->is_low((enum Input::input_channels)ch) ); } +void Input::print() { + debug("menu %d (old %d)\n", this->curr->menu, this->old->menu); + + for (uint8_t i = 0; i < CH_COUNT; ++i) { + print_ch((enum Input::input_channels) i); + } + for (uint8_t i = 0; i < NUM_TX_CHN; ++i) { + debug("chdata %d \n", this->channel_data[i]); + } +} + bool Input::calibration_update(void) { bool changed = false; @@ -201,12 +208,14 @@ void Input::get_calibration(struct ch_config *curr_config) } void Input::update(void) { + this->mark_processed(); for (uint8_t ch = 0; ch < CH_MAX; ch ++) { - if (this->ch_config[ch].is_analog) + if (this->ch_config[ch].is_analog) { this->ch_raw[ch] = analogRead(this->pins[ch]); - else + } else { this->ch_raw[ch] = digitalRead(this->pins[ch]) == HIGH; + } // do inverting if (this->ch_config[ch].inverted) @@ -234,4 +243,3 @@ void Input::update(void) { this->curr->aux[4],this->curr->aux[5],this->curr->menu );*/ } -// Channel value for FrSky (PPM is multiplied by 1.5) diff --git a/remote/src/state_bind.cpp b/remote/src/state_bind.cpp index 8561766..8619699 100644 --- a/remote/src/state_bind.cpp +++ b/remote/src/state_bind.cpp @@ -54,7 +54,6 @@ void LCD_state_bind::update(void) lcd.setCursor(14,1); lcd.print(line); - uint32_t wait_until = start + next_callback_time; end__ = micros(); if (end__ - start < next_callback_time) { diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp index e61baa0..06ca903 100644 --- a/remote/src/state_fly.cpp +++ b/remote/src/state_fly.cpp @@ -7,30 +7,20 @@ #include "eeprom.h" #include "debug.h" #include "tx_def.h" -#include "common.h" 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 "); - lcd.setCursor(0,1); - lcd.print(" "); this->time_enter = millis(); -} - -void LCD_state_fly::print_time(uint16_t time) -{ - /** - * 0123456789012345 - * fly mode A PP - * T SSS AA PP A PP - **/ - char line[17]; - lcd.setCursor(0,1); - byte clock_char_data[8] = { + byte clock_char_data[] = { 0b01110, 0b10101, 0b10101, @@ -41,9 +31,52 @@ void LCD_state_fly::print_time(uint16_t time) 0b00000 }; lcd.createChar(clock_char, clock_char_data); + lcd.setCursor(0,1); lcd.write(clock_char); - snprintf(line, sizeof(line), "%*d", 3, time); + byte rssi_antenna_[] = { + 0b10101, + 0b10001, + 0b01110, + 0b00100, + 0b00100, + 0b00100, + 0b00101, + 0b00101, }; + lcd.createChar(rssiantenna, rssi_antenna_); + lcd.setCursor(6,1); + lcd.write(rssiantenna); + + byte rssi_bars_[] = { + 0b00001, + 0b00001, + 0b00001, + 0b00001, + 0b00101, + 0b00101, + 0b10101, + 0b10101 }; + lcd.createChar(rssi_bars, rssi_bars_); + lcd.setCursor(7,1); + lcd.write(rssi_bars); +} + +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(uint8_t akku_quad, uint8_t akku_remote) @@ -57,8 +90,7 @@ void LCD_state_fly::print_akku(uint8_t akku_quad, uint8_t akku_remote) char line[17]; uint8_t akku[] = {akku_remote, akku_quad}; - for (uint8_t i = 0; i < 2; ++i) { - lcd.setCursor(12,i); + 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; @@ -67,44 +99,29 @@ void LCD_state_fly::print_akku(uint8_t akku_quad, uint8_t akku_remote) 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, battery_char_data); - lcd.write(battery_char); - snprintf(line, sizeof(line), " %*d", 2, akku[i]); + 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); } + return; } void LCD_state_fly::print_rssi(uint8_t rssi_percent) { char line[17]; - byte rssi_antenna[8] = { 0b10101, 0b10101, 0b01110, 0b00100, 0b00100, 0b00101, 0b00101, 0b00101 }; - - byte rssi_antenna_1[8] = { 0b00001, 0b00001, 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101 }; - byte rssi_antenna_2[8] = { 0b00001, 0b00001, 0b00001, 0b00001, 0b10101, 0b10101, 0b10101, 0b10101 }; - byte rssi_antenna_3[8] = { 0b00001, 0b00001, 0b00101, 0b00101, 0b10101, 0b10101, 0b10101, 0b10101 }; - lcd.setCursor(6,1); - lcd.createChar(rssiantenna, rssi_antenna); - - lcd.write(rssiantenna); - if (rssi_percent > 75) {/* 100-75 -> 3 */ - lcd.createChar(rssi_bars, rssi_antenna_3); - lcd.write(rssi_bars); - } else if (rssi_percent > 50) { /* 74-50 -> 2 */ - lcd.createChar(rssi_bars, rssi_antenna_2); - lcd.write(rssi_bars); - } else if (rssi_percent > 25) { /* 49-25 -> 1 */ - lcd.createChar(rssi_bars, rssi_antenna_1); - lcd.write(rssi_bars); - } else { /* 25-0 -> 0 */ - lcd.write(' '); - } - - snprintf(line, sizeof(line), " %*d", 2, rssi_percent); + 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; @@ -112,37 +129,71 @@ void LCD_state_fly::update(void) unsigned long time_in_ms = millis() - this->time_enter; unsigned long time_in_s = time_in_ms/1000; // to sec - snprintf(line, sizeof(line), "fly mode %02lu sec", time_in_s); - lcd.setCursor(0,1); - lcd.print(line); + this->print_time(time_in_s); + this->print_akku(akku_quad, akku_remote); + this->print_rssi(rssi_percent); 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 (state == FRSKY_DATA1) { - // update time - time_in_ms = millis() - this->time_enter; - time_in_s = time_in_ms/1000; // to sec + //if (next_callback_time > 9000) { + if (next_callback_time > 9000) { + input.update(); - // print on lcd - this->print_time(time_in_s); + if (input.is_menu_triggered()) { + debug("%lu menu button trigger\n", millis()); + input.print(); + break; + } - rssi_percent += 1; - this->print_time(rssi_percent); - akku_quad += 1; - akku_remote += 2; - this->print_akku(akku_quad, akku_remote); + // print on lcd + call +=1; + if(call > 5) + call= 0; + + switch(call) + { + case 1: + rssi_percent += 1; + if(rssi_percent > 100) + rssi_percent = 0; + this->print_rssi(rssi_percent); + break; + + case 2: + // 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: + // update akku + akku_quad += 1; + akku_remote += 2; + if(akku_quad > 100) + akku_quad = 0; + if(akku_remote > 100) + akku_remote = 0; + this->print_akku(akku_quad, akku_remote); + break; + + default: + break; + } - input.update(); } @@ -153,10 +204,6 @@ void LCD_state_fly::update(void) delayMicroseconds(wait); } end__ = micros(); - - if (input.is_menu_triggered() == 0) - break; - } // cange to menu when done diff --git a/remote/src/state_init.cpp b/remote/src/state_init.cpp index c7db9d9..98de2e9 100644 --- a/remote/src/state_init.cpp +++ b/remote/src/state_init.cpp @@ -44,6 +44,12 @@ void LCD_state_init::update(void) 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) diff --git a/remote/src/state_menu.cpp b/remote/src/state_menu.cpp index 17ba407..c76d8c1 100644 --- a/remote/src/state_menu.cpp +++ b/remote/src/state_menu.cpp @@ -41,6 +41,7 @@ void LCD_state_menu::update(void) 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; From 84b458f1ef108a5a0f3830cf4d977be42beb9029 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 13:43:41 +0200 Subject: [PATCH 06/10] 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 **/ +/**************************/ +/**************************/ From ff400bfb7b6c0897eba236b67a03b4fb73884264 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 15:21:38 +0200 Subject: [PATCH 07/10] fix inverting of channels --- remote/src/input.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/remote/src/input.cpp b/remote/src/input.cpp index e483276..ce62986 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -234,7 +234,7 @@ void Input::update(void) { for (uint8_t ch = 0; ch < CH_COUNT; ++ch) { - this->channel_data[ch] = map(this->curr->ch_data[ch], this->ch_config[ch].min, this->ch_config[ch].max, CHANNEL_MAX_100, CHANNEL_MIN_100); + this->channel_data[ch] = map(this->curr->ch_data[ch], this->ch_config[ch].min, this->ch_config[ch].max, CHANNEL_MIN_100, CHANNEL_MAX_100); } /*debug_input("t%d y%d r%d p%d a1_%d a2_%d a3_%d a4_%d a5_%d m%d", From 1ce848b5d36ab881cc9d640fae4ad05a4a17a142 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 15:22:08 +0200 Subject: [PATCH 08/10] remove logs and reduce bootup time --- remote/src/state_fly.cpp | 8 ++++---- remote/src/state_init.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp index dfd7e51..fb07818 100644 --- a/remote/src/state_fly.cpp +++ b/remote/src/state_fly.cpp @@ -217,8 +217,6 @@ void LCD_state_fly::update(void) akku_remote = 0; this->print_akku_remote(akku_remote); break; - case 50: - //print_frskyd_telemetry(); default: break; @@ -232,8 +230,10 @@ 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)); + /* 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(); } diff --git a/remote/src/state_init.cpp b/remote/src/state_init.cpp index 98de2e9..2530d30 100644 --- a/remote/src/state_init.cpp +++ b/remote/src/state_init.cpp @@ -22,7 +22,7 @@ void LCD_state_init::update(void) uint32_t diff; delay(100); diff = millis() - this->time_enter; - if (diff > 5 * 1000) { + if (diff > 3 * 1000) { new_state = s_joy; debugln("read start"); delay(1000); From 75e5543ecf6d6423a590d5d7371e548ad38eda8e Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 15:24:35 +0200 Subject: [PATCH 09/10] add usb state class --- remote/include/state.h | 11 ++++++++ remote/src/FrSkyD_cc2500.cpp | 3 +++ remote/src/state.cpp | 2 ++ remote/src/state_joy_usb.cpp | 52 ++++++++++++++++++++++++++++++++++++ remote/src/state_menu.cpp | 1 + 5 files changed, 69 insertions(+) create mode 100644 remote/src/state_joy_usb.cpp diff --git a/remote/include/state.h b/remote/include/state.h index 23ddd41..f7f1e2b 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -93,6 +93,16 @@ public: void update(void); void leave(void); }; + +class LCD_state_joy_usb: public State { +private: + unsigned long time_enter; +public: + LCD_state_joy_usb(void); + void enter(void); + void update(void); + void leave(void); +}; extern State *curr_state; extern State *new_state; @@ -100,5 +110,6 @@ extern State *s_init; extern State *s_bind; extern State *s_fly; extern State *s_joy; +extern State *s_usb; extern State *s_menu; #endif /*_STATE_H_*/ diff --git a/remote/src/FrSkyD_cc2500.cpp b/remote/src/FrSkyD_cc2500.cpp index 9af091e..af77df4 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -245,13 +245,16 @@ uint16_t ReadFrSky_2way() //debugln("%d len",len); if (len && len<=(0x11+3)) { // 20bytes + debug("rx tel\n"); CC2500_ReadData(pkt, len); //received telemetry packets #if defined(TELEMETRY) if(pkt[len-1] & 0x80) {//with valid crc + debug("rx invalid crc\n"); packet_count=0; frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them. } + debug("rx end crc\n"); #endif } else { packet_count++; diff --git a/remote/src/state.cpp b/remote/src/state.cpp index bd3b669..dc13aed 100644 --- a/remote/src/state.cpp +++ b/remote/src/state.cpp @@ -17,6 +17,7 @@ State *s_init = NULL; State *s_bind = NULL; State *s_fly = NULL; State *s_joy = NULL; +State *s_usb = NULL; State *s_menu = NULL; @@ -32,6 +33,7 @@ void init_state(void) { s_bind = new LCD_state_bind(); s_fly = new LCD_state_fly(); s_joy = new LCD_state_joy_calibration(); + s_usb = new LCD_state_joy_usb(); s_menu = new LCD_state_menu(); lcd.backlight(); curr_state = NULL; diff --git a/remote/src/state_joy_usb.cpp b/remote/src/state_joy_usb.cpp new file mode 100644 index 0000000..8a6db0d --- /dev/null +++ b/remote/src/state_joy_usb.cpp @@ -0,0 +1,52 @@ +#include +#include +#include "Arduino.h" +#include "state.h" +#include "input.h" +#include "debug.h" + +//#include + +/* USBHID* HID; */ +/* 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); + + /* HID = new USBHID(); */ + /* Joystick = new HIDJoystick(*HID); */ + /* HID->begin(HID_JOYSTICK); */ +} +void LCD_state_joy_usb::update(void) { + + input.update(); + input.print(); + while(1) { + /* Joystick->X(0); */ + /* delay(500); */ + /* Joystick->X(1023); */ + /* delay(500); */ + + input.update(); + + if (input.is_menu_triggered()) { + debug("%lu menu button trigger\n", millis()); + input.print(); + break; + } + } + 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 "); +} diff --git a/remote/src/state_menu.cpp b/remote/src/state_menu.cpp index c76d8c1..c3fae82 100644 --- a/remote/src/state_menu.cpp +++ b/remote/src/state_menu.cpp @@ -26,6 +26,7 @@ void LCD_state_menu::update(void) } menus[] = { { "Flight ", s_fly }, { "Bind ", s_bind }, + { "Joy usb ", s_usb}, { "Joy calib ", s_joy }, { "HF calib ", NULL }, { " ", NULL }, From cd38d9cd066277c9cdc7fb32d584a3c44adaed09 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Thu, 4 Apr 2019 16:55:59 +0200 Subject: [PATCH 10/10] implement usb --- remote/include/debug.h | 2 +- remote/include/input.h | 3 +- remote/src/Multiprotocol.cpp | 5 +++ remote/src/input.cpp | 20 +++++++---- remote/src/state_fly.cpp | 52 ++++++++++++++++------------- remote/src/state_joy_usb.cpp | 64 +++++++++++++++++++++--------------- 6 files changed, 89 insertions(+), 57 deletions(-) diff --git a/remote/include/debug.h b/remote/include/debug.h index f511621..a0f0036 100644 --- a/remote/include/debug.h +++ b/remote/include/debug.h @@ -6,7 +6,7 @@ //******************** //** Debug messages ** //******************** -#define ENABLE_DBEUG // ~1k +//#define ENABLE_DBEUG // ~1k #ifdef ENABLE_DBEUG #define debug(msg, ...) { char buf[256]; sprintf(buf, msg, ##__VA_ARGS__); Serial.print(buf);} #define debugln(msg, ...) { char buf[256]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} diff --git a/remote/include/input.h b/remote/include/input.h index 95c6db9..e379d02 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -57,10 +57,11 @@ class Input { void get_calibration(struct ch_config *curr_config); uint16_t *get_channel_data(void); + + uint16_t ch_raw[CH_COUNT]; private: // raw sticks input - uint16_t ch_raw[CH_COUNT]; // calculated inputs (my be inverted struct data { diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index a2c0f45..c7df700 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -66,6 +66,10 @@ uint8_t crc8; uint16_t failsafe_count; uint8_t len; +#include +USBHID HID; +HIDJoystick Joystick(HID); + // Telemetry void setup() { @@ -78,6 +82,7 @@ void setup() debugln("Multiprotocol startup"); debugln("time %s ", __TIME__); #endif + HID.begin(HID_JOYSTICK); // all inputs // outputs diff --git a/remote/src/input.cpp b/remote/src/input.cpp index ce62986..820c501 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -88,10 +88,11 @@ void Input::init() { this->ch_config[CH_YAW].inverted = false; this->ch_config[CH_ROLL].inverted = false; this->ch_config[CH_PITCH].inverted = false; - this->ch_config[CH_AUX1].inverted = false; - this->ch_config[CH_AUX2].inverted = false; - this->ch_config[CH_AUX3].inverted = false; - this->ch_config[CH_AUX4].inverted = false; + this->ch_config[CH_AUX1].inverted = true; + this->ch_config[CH_AUX2].inverted = true; + this->ch_config[CH_AUX3].inverted = true; + this->ch_config[CH_AUX4].inverted = true; + this->ch_config[CH_AUX5].inverted = true; } bool Input::is_centered(void) { @@ -190,8 +191,13 @@ bool Input::calibration_update(void) { } void Input::calibration_reset(void) { for (uint8_t ch = 0; ch < CH_COUNT; ch++) { - this->ch_config[ch].min = this->ch_raw[ch]; - this->ch_config[ch].max = this->ch_raw[ch]; + if (this->ch_config[ch].is_analog) { + this->ch_config[ch].min = this->ch_raw[ch]; + this->ch_config[ch].max = this->ch_raw[ch]; + }else { + this->ch_config[ch].min = CHANNEL_MIN_100; + this->ch_config[ch].max = CHANNEL_MAX_100; + } } } void Input::set_calibration(struct ch_config *new_config) @@ -214,7 +220,7 @@ void Input::update(void) { if (this->ch_config[ch].is_analog) { this->ch_raw[ch] = analogRead(this->pins[ch]); } else { - this->ch_raw[ch] = digitalRead(this->pins[ch]) == HIGH; + this->ch_raw[ch] = digitalRead(this->pins[ch]) == HIGH ? CHANNEL_MIN_100 : CHANNEL_MAX_100; } // do inverting diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp index fb07818..562f872 100644 --- a/remote/src/state_fly.cpp +++ b/remote/src/state_fly.cpp @@ -8,6 +8,9 @@ #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) { } @@ -45,8 +48,6 @@ void LCD_state_fly::enter(void) { 0b00101, 0b00101, }; lcd.createChar(rssiantenna, rssi_antenna_); - lcd.setCursor(6,1); - lcd.write(rssiantenna); byte rssi_bars_[8]; @@ -61,8 +62,6 @@ void LCD_state_fly::enter(void) { lcd.createChar(rssi_bars, rssi_bars_); - lcd.setCursor(7,1); - lcd.write(rssi_bars); byte battery_char_data[8]; battery_char_data[0] = 0b01110; @@ -75,11 +74,16 @@ void LCD_state_fly::enter(void) { 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); +#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) @@ -149,11 +153,6 @@ void LCD_state_fly::update(void) 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_quad(akku_quad); - this->print_akku_remote(akku_remote); - this->print_rssi(rssi_percent); - uint32_t end__ = micros(); uint32_t start = micros(); uint32_t next_callback_time; @@ -188,20 +187,23 @@ void LCD_state_fly::update(void) switch(call) { - case 10: - rssi_percent += 1; - if(rssi_percent > 100) - rssi_percent = 0; - this->print_rssi(rssi_percent); - break; - case 20: + + 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; @@ -210,11 +212,17 @@ void LCD_state_fly::update(void) this->print_akku_quad(akku_quad); break; +#endif case 40: // update akku - akku_remote += 2; - if(akku_remote > 100) - akku_remote = 0; + 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; diff --git a/remote/src/state_joy_usb.cpp b/remote/src/state_joy_usb.cpp index 8a6db0d..2a10187 100644 --- a/remote/src/state_joy_usb.cpp +++ b/remote/src/state_joy_usb.cpp @@ -5,13 +5,10 @@ #include "input.h" #include "debug.h" -//#include - -/* USBHID* HID; */ -/* HIDJoystick* Joystick; */ - +int16_t map16b( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); +#include +extern HIDJoystick Joystick; LCD_state_joy_usb::LCD_state_joy_usb(void) { - } void LCD_state_joy_usb::enter(void) { lcd.setCursor(0,0); @@ -19,30 +16,45 @@ void LCD_state_joy_usb::enter(void) { lcd.setCursor(0,1); lcd.print(" "); delay(500); - - /* HID = new USBHID(); */ - /* Joystick = new HIDJoystick(*HID); */ - /* HID->begin(HID_JOYSTICK); */ } void LCD_state_joy_usb::update(void) { - input.update(); - input.print(); - while(1) { - /* Joystick->X(0); */ - /* delay(500); */ - /* Joystick->X(1023); */ - /* delay(500); */ - - input.update(); - - if (input.is_menu_triggered()) { - debug("%lu menu button trigger\n", millis()); - input.print(); - break; - } + + 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; } - new_state = s_menu; } void LCD_state_joy_usb::leave(void) { lcd.setCursor(0,0);