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 ) { diff --git a/remote/include/FrSkyD_cc2500.h b/remote/include/FrSkyD_cc2500.h index da8c7ee..dfbefaa 100644 --- a/remote/include/FrSkyD_cc2500.h +++ b/remote/include/FrSkyD_cc2500.h @@ -17,6 +17,37 @@ #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 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; + +void Frsky_init_hop(void); +void FRSKY_init_cc2500(const uint8_t *ptr); + void frsky2way_init(uint8_t bind); void frsky2way_build_bind_packet(); @@ -25,7 +56,8 @@ 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); +void set_rx_tx_addr(uint32_t id); #endif diff --git a/remote/include/Multiprotocol.h b/remote/include/Multiprotocol.h index 0ae59e9..aef0d0e 100644 --- a/remote/include/Multiprotocol.h +++ b/remote/include/Multiprotocol.h @@ -24,25 +24,14 @@ #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 hopping_frequency[50]; 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; @@ -75,9 +64,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) @@ -89,7 +76,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/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/common.h b/remote/include/common.h deleted file mode 100644 index 0a95036..0000000 --- a/remote/include/common.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - This project is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Multiprotocol is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Multiprotocol. If not, see . -*/ -#ifndef _COMMON_H_ -#define _COMMON_H_ -#include -#include "config.h" -#include "tx_def.h" - -void InitChannel(void); -void reverse_channel(uint8_t num); -uint16_t convert_channel_ppm(uint8_t num); -uint16_t convert_channel_10b(uint8_t num); -uint8_t convert_channel_8b(uint8_t num); -int16_t convert_channel_16b_limit(uint8_t num, int16_t min, int16_t max); -int16_t convert_channel_16b_nolimit(uint8_t num, int16_t min, int16_t max); -uint8_t convert_channel_s8b(uint8_t num); -uint16_t limit_channel_100(uint8_t num); -void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high); -void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high); -uint16_t convert_channel_frsky(uint8_t num); - -/******************************/ -/** FrSky D and X routines **/ -/******************************/ -enum { - FRSKY_BIND = 0, - FRSKY_BIND_DONE = 1000, - FRSKY_DATA1, - FRSKY_DATA2, - FRSKY_DATA3, - FRSKY_DATA4, - FRSKY_DATA5 -}; - -void Frsky_init_hop(void); -/******************************/ -/** FrSky V, D and X routines **/ -/******************************/ -#if defined(FRSKYV_CC2500_INO) || defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) - - extern uint8_t FRSKY_common_startreg_cc2500_conf[]; - - extern uint8_t FRSKYD_cc2500_conf[]; - - - extern uint8_t FRSKY_common_end_cc2500_conf[][2]; - void FRSKY_init_cc2500(const uint8_t *ptr); - -#endif - -#endif diff --git a/remote/include/config.h b/remote/include/config.h index e903306..310b467 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,39 +162,7 @@ /*************************/ /*** 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; struct PPM_Parameters { @@ -228,47 +187,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 new file mode 100644 index 0000000..4193f78 --- /dev/null +++ b/remote/include/crc.h @@ -0,0 +1,5 @@ +#ifndef __CRC32_H_ +#define __CRC32_H_ +#include +uint32_t tiny_crc32(const void *data, unsigned int length); +#endif 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/eeprom.h b/remote/include/eeprom.h index 0a43387..3c65cc7 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; - -}; - - -const static uint32_t crc_table[16] = { - 0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac, - 0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c, - 0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c, - 0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c -}; - -uint32_t crc_update (uint32_t crc, uint8_t data) -{ - uint8_t tbl_idx; - - tbl_idx = crc ^ (data >> (0 * 4)); - crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); +class Eeprom_config { +public: - tbl_idx = crc ^ (data >> (1 * 4)); - crc = (crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4); + Eeprom_config(void); + ~Eeprom_config(void); - return crc; -} + int validate(void); + int read(void); + int write(void); + int get_ch_config(struct Input::ch_config* config); + int set_ch_config(struct Input::ch_config* config); -int read_eeprom(struct eeprom_data_v1 *eeprom_data) { - uint8_t *data = NULL; - uint8_t start_address = 0x10; + int get_master_id(uint32_t* master_id); + int set_master_id(uint32_t master_id); - 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; - } +private: + #define CURRENT_VERSION 0x02 + struct eeprom_data_v1 { + uint8_t version; + uint32_t data_crc; + struct { + uint32_t master_id; + struct Input::ch_config ch[Input::CH_COUNT]; + } data; + } current_config; + bool sucessfull_read; +}; - // validate crc - if (eeprom_data->crc != crc) { - return -1; - } -} #endif diff --git a/remote/include/input.h b/remote/include/input.h index 22484e2..e379d02 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -5,9 +5,6 @@ #define NUM_TX_CHN 16 -extern uint16_t Channel_data[NUM_TX_CHN]; -extern uint16_t Failsafe_data[NUM_TX_CHN]; -extern const char* ch_name[NUM_TX_CHN]; class Input { public: enum input_channels { @@ -21,7 +18,6 @@ class Input { CH_AUX3 = 6, CH_AUX4 = 7, CH_AUX5 = 8, - CH_AUX6 = 8, CH_MAX = 8, CH_COUNT = 9, @@ -29,20 +25,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 +48,42 @@ class Input { void invert_ch(enum input_channels ch); void print_ch(enum input_channels ch); - void calibration_init(void); + void print(); + + 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); + + uint16_t ch_raw[CH_COUNT]; private: + + // raw sticks input + + // calculated inputs (my be inverted + struct data { + uint16_t ch_data[CH_COUNT]; + bool menu; + }; + + // actual tx channel data + uint16_t channel_data[NUM_TX_CHN]; + uint16_t failsafe_data[NUM_TX_CHN]; + 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]; - uint32_t pins[CH_COUNT]; + // pin setttings + int pins[CH_COUNT]; }; - -extern uint16_t Channel_data[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 c05d4db..f7f1e2b 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -5,6 +5,16 @@ #include #include +extern LiquidCrystal_I2C lcd; +enum lcd_special_chars { + battery_char= 0, + + rssiantenna= 3, + rssi_bars = 4, + clock_char = 5, + MAX_SPECIAL_CHARS =8, +}; + void init_state(void); void update_state(void); @@ -52,6 +62,12 @@ public: class LCD_state_fly: public State { private: unsigned long time_enter; + uint16_t last_time; + + 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); public: LCD_state_fly(void); @@ -77,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; @@ -84,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/include/telemetry.h b/remote/include/telemetry.h new file mode 100644 index 0000000..42230f1 --- /dev/null +++ b/remote/include/telemetry.h @@ -0,0 +1,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); +#endif + +#ifdef MULTI_TELEMETRY +static void multi_send_frskyhub(); +#endif + + +void frsky_check_telemetry(uint8_t *pkt,uint8_t len); + +void print_frskyd_telemetry(); +void init_frskyd_link_telemetry(); +#endif 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 3aa90d4..af77df4 100644 --- a/remote/src/FrSkyD_cc2500.cpp +++ b/remote/src/FrSkyD_cc2500.cpp @@ -13,27 +13,111 @@ along with Multiprotocol. If not, see . */ -#include "common.h" #include "cc2500_spi.h" #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; + +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"); + //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); //#######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 @@ -43,7 +127,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++]; @@ -80,17 +164,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)); } @@ -104,7 +184,6 @@ uint16_t initFrSky_2way() if(IS_BIND_IN_PROGRESS) { - frsky2way_init(1); state = FRSKY_BIND; debugln("initFrSky_2way bind"); } else { @@ -113,43 +192,45 @@ uint16_t initFrSky_2way() } return 10000; } +uint16_t ReadFrSky_2way_bind(void) +{ + 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 { + state++; + } + + if(state == FRSKY_BIND_DONE) { + /* reset state again */ + state = 0; + debugln("%s bind done fr",__func__); + } + return 9000; +} uint16_t ReadFrSky_2way() { - //debugln("%s",__func__); + if (state <= FRSKY_BIND_DONE) { - 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(state == FRSKY_BIND_DONE) { - debugln("%s bind done fr",__func__); - } - return 9000; - } - if (state == FRSKY_BIND_DONE) { - //debugln("%s bind done",__func__); + frsky2way_init(0); + //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; - } + 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); @@ -162,15 +243,18 @@ 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 + 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++; @@ -188,10 +272,13 @@ 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(); @@ -200,3 +287,66 @@ 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; + } +} + +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 f1b9d6a..c7df700 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,20 +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]; - -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; @@ -66,470 +59,72 @@ 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; -uint8_t throttle, rudder, elevator, aileron; -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) - uint8_t calData[48]; -#endif - - -// 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; -#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; +#include +USBHID HID; +HIDJoystick Joystick(HID); // 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); -static void protocol_init(); -uint8_t Update_All(); - -// 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 + HID.begin(HID_JOYSTICK); - // 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); - -#ifdef ENABLE_PPM - //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); - 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 - } - line++; - - protocol_init(); + freq_offset = 0; + debug("freq offset: %d\n", freq_offset); + CC2500_Reset(); + //Wait for cc2500 to reset + delay(100); } -#endif //ENABLE_PPM - debug("Init complete\n"); - input.init(); - input.update(); - init_state(); - -} - -// Main -// Protocol scheduler -void loop() -{ - uint32_t s; - s =micros(); + input.init(); input.update(); - debug("input took %lu", (micros()-s)); - - s =micros(); - update_state(); - debugln("state took %lu", (micros()-s)); - 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(); - - 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)); - } + init_state(); - 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__ += wait; - } - end__ = micros(); - } + debug("Init complete\n"); } -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); - - blink=millis(); - - 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; - } - } - - #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() -{ - 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) +void loop() { - #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 - + update_state(); } 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/common.cpp b/remote/src/common.ds similarity index 53% rename from remote/src/common.cpp rename to remote/src/common.ds index 88606a7..1b3dc60 100644 --- a/remote/src/common.cpp +++ b/remote/src/common.ds @@ -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 **/ @@ -264,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/crc.cpp b/remote/src/crc.cpp new file mode 100644 index 0000000..133557b --- /dev/null +++ b/remote/src/crc.cpp @@ -0,0 +1,28 @@ +#include "crc.h" + +static const unsigned int tiny_crc32tab[16] = { + 0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac, + 0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c, + 0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c, + 0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c +}; + + +uint32_t __tiny_crc32(const void *data, unsigned int length, uint32_t crc) +{ + const unsigned char *buf = (const unsigned char *)data; + unsigned int i; + + for (i = 0; i < length; ++i) { + crc ^= buf[i]; + crc = tiny_crc32tab[crc & 0x0f] ^ (crc >> 4); + crc = tiny_crc32tab[crc & 0x0f] ^ (crc >> 4); + } + + return crc; +} +uint32_t tiny_crc32(const void *data, unsigned int length) +{ + // return value suitable for passing in next time, for final value invert it + return __tiny_crc32(data, length, 42) ^ 0xffffff; +} diff --git a/remote/src/eeprom.cpp b/remote/src/eeprom.cpp new file mode 100644 index 0000000..14bae0f --- /dev/null +++ b/remote/src/eeprom.cpp @@ -0,0 +1,107 @@ +#include + +#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() { + uint32_t crc_calc = 0; + + if (this->sucessfull_read == false) { + debugln("no read\n"); + return -1; + } + + if (this->current_config.version != CURRENT_VERSION) { + debugln("wrong version %lu vs %lu \n", this->current_config.version, CURRENT_VERSION); + return -1; + } + + crc_calc = tiny_crc32(&(this->current_config.data), sizeof(this->current_config.data)); + + if (crc_calc != this->current_config.data_crc) { + debugln("wrong crc %lu vs %lu \n", crc_calc, this->current_config.data_crc); + return -1; + } + debugln("valid config\n"); + return 1; +} + +int Eeprom_config::read(void) { + uint8_t *data = NULL; + data = (uint8_t *) &this->current_config; + + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { + data[i] = EEPROM.read(0x10 + i); + } + + this->sucessfull_read = true; + return 0; +} + +int Eeprom_config::write(void) { + uint8_t *data = NULL; + data = (uint8_t *) &this->current_config; + + this->current_config.version = CURRENT_VERSION; + this->current_config.data_crc = tiny_crc32(&(this->current_config.data), sizeof(this->current_config.data)); + + for (uint8_t i = 0; i < sizeof(struct eeprom_data_v1) ; i++) { + EEPROM.write(0x10 + i , data[i]); + } + + return 0; +} + +int Eeprom_config::get_ch_config(struct Input::ch_config* config) { + if (this->sucessfull_read == false && config != NULL) { + return -1; + } + memcpy(config, &this->current_config.data.ch, sizeof(struct Input::ch_config) * Input::CH_COUNT); + return 0; +} + +int Eeprom_config::set_ch_config(struct Input::ch_config* config) { + if (this->sucessfull_read == false && config != NULL) { + return -1; + } + memcpy(this->current_config.data.ch, config, sizeof(struct Input::ch_config) * Input::CH_COUNT); + return 0; +} + +int Eeprom_config::get_master_id(uint32_t* master_id) +{ + if (this->sucessfull_read == false && master_id != NULL) { + return -1; + } + *master_id = this->current_config.data.master_id; + return 0; +} + +int Eeprom_config::set_master_id(uint32_t master_id) +{ + if (this->sucessfull_read == false) { + return -1; + } + this->current_config.data.master_id = master_id; + return 0; +} diff --git a/remote/src/input.cpp b/remote/src/input.cpp index 807303f..820c501 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -8,10 +8,8 @@ #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] = { +const char* ch_name[Input::CH_COUNT] = { "CH_ROLL", "CH_PITCH", "CH_THROTTLE", @@ -22,23 +20,27 @@ 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++) - 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 + for (i = 0; i < NUM_TX_CHN; i++) { + //InitFailsafe + this->failsafe_data[i] = (CHANNEL_MAX_100 - CHANNEL_MIN_100) / 2 + CHANNEL_MIN_100; + // init channel + this->channel_data[i] = 1024; + } + + this->failsafe_data[CH_THROTTLE] = CHANNEL_MIN_100; //1=-125%, 204=-100% + this->channel_data[CH_THROTTLE] = CHANNEL_MIN_100; +} - for (uint8_t i = 0; i < NUM_TX_CHN; i++) - Channel_data[i] = 1024; - 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; @@ -46,80 +48,65 @@ 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; - - this->pins[CH_YAW] = Yaw_pin; - this->ch_config[CH_YAW].is_analog = true; + this->pins[CH_THROTTLE] = Throttle_pin; + this->ch_config[CH_THROTTLE].is_analog = true; - this->pins[CH_PITCH] = Pitch_pin; - this->ch_config[CH_PITCH].is_analog = true; + this->pins[CH_YAW] = Yaw_pin; + this->ch_config[CH_YAW].is_analog = true; - this->pins[CH_ROLL] = Roll_pin; - this->ch_config[CH_ROLL].is_analog = true; + this->pins[CH_PITCH] = Pitch_pin; + this->ch_config[CH_PITCH].is_analog = true; - this->pins[CH_AUX1] = Aux1_pin; - this->ch_config[CH_AUX1].is_analog = false; + this->pins[CH_ROLL] = Roll_pin; + this->ch_config[CH_ROLL].is_analog = true; - this->pins[CH_AUX2] = Aux2_pin; - this->ch_config[CH_AUX2].is_analog = false; + this->pins[CH_AUX1] = Aux1_pin; + this->ch_config[CH_AUX1].is_analog = false; - this->pins[CH_AUX3] = Aux3_pin; - this->ch_config[CH_AUX3].is_analog = false; + this->pins[CH_AUX2] = Aux2_pin; + this->ch_config[CH_AUX2].is_analog = false; - this->pins[CH_AUX4] = Aux4_pin; - this->ch_config[CH_AUX4].is_analog = false; + this->pins[CH_AUX3] = Aux3_pin; + this->ch_config[CH_AUX3].is_analog = false; - this->pins[CH_AUX5] = Aux5_pin; - this->ch_config[CH_AUX5].is_analog = false; + this->pins[CH_AUX4] = Aux4_pin; + this->ch_config[CH_AUX4].is_analog = false; - this->pins[CH_AUX6] = Aux6_pin; - this->ch_config[CH_AUX6].is_analog = false; + this->pins[CH_AUX5] = Aux5_pin; + this->ch_config[CH_AUX5].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 = 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) { - 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,35 +116,36 @@ 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; } 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) { 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), @@ -165,6 +153,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; @@ -190,49 +189,63 @@ 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]; + 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) +{ + 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)); } } -void Input::update(void) { - for (uint8_t ch = 0; ch < CH_MAX; ch ++) { +void Input::update(void) { + this->mark_processed(); + 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; + 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 ? CHANNEL_MIN_100 : CHANNEL_MAX_100; + } - // 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]; + // 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_MIN_100, CHANNEL_MAX_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); - } } diff --git a/remote/src/state.cpp b/remote/src/state.cpp index eb50493..dc13aed 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" @@ -16,50 +17,10 @@ State *s_init = NULL; State *s_bind = NULL; State *s_fly = NULL; State *s_joy = NULL; +State *s_usb = 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); @@ -72,12 +33,12 @@ 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; new_state = s_init; - install_special_caracters(); update_state(); } @@ -95,295 +56,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_init(); - - // 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; - } - } - } - - 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..8619699 --- /dev/null +++ b/remote/src/state_bind.cpp @@ -0,0 +1,78 @@ +#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) +{ + char line[17]; + unsigned long time_in_ms = millis() - this->time_enter; + unsigned long time_in_s = time_in_ms/1000; // to sec + unsigned long remain_s = this->bind_time - time_in_s; + + snprintf(line,sizeof(line),"remaining sec %02lu",remain_s); + lcd.setCursor(0,1); + lcd.print(line); + + uint32_t end__ = micros(); + uint32_t start = micros(); + uint32_t next_callback_time; + next_callback_time = initFrSky_2way(); + + // init for bind + frsky2way_init(1); + while(1) { + start = end__; + next_callback_time = ReadFrSky_2way_bind(); + + + // update time + time_in_ms = millis() - this->time_enter; + time_in_s = time_in_ms/1000; // to sec + remain_s = this->bind_time - time_in_s; + + // print on lcd + snprintf(line,sizeof(line),"%02lu",remain_s); + lcd.setCursor(14,1); + lcd.print(line); + + end__ = micros(); + + if (end__ - start < next_callback_time) { + uint32_t wait = next_callback_time; + wait -= ((end__-start)); + delayMicroseconds(wait); + end__ += wait; + } + end__ = micros(); + + if (remain_s == 0) + break; + } + + // cange to menu when done + new_state = s_menu; +} + +void LCD_state_bind::leave(void) +{ + lcd.clear(); +} diff --git a/remote/src/state_fly.cpp b/remote/src/state_fly.cpp new file mode 100644 index 0000000..562f872 --- /dev/null +++ b/remote/src/state_fly.cpp @@ -0,0 +1,256 @@ +#include +#include +#include "Arduino.h" +#include "FrSkyD_cc2500.h" +#include "telemetry.h" +#include "state.h" +#include "input.h" +#include "eeprom.h" +#include "debug.h" +#include "tx_def.h" +#include "pins.h" + +int16_t map16b(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); + +LCD_state_fly::LCD_state_fly(void) { +} + +void LCD_state_fly::enter(void) { + + this->last_time = 0; + + lcd.clear(); + lcd.setCursor(0,0); + lcd.print("fly mode "); + this->time_enter = millis(); + + byte clock_char_data[] = { + 0b01110, + 0b10101, + 0b10101, + 0b10111, + 0b10001, + 0b10001, + 0b01110, + 0b00000 + }; + lcd.createChar(clock_char, clock_char_data); + lcd.setCursor(0,1); + lcd.write(clock_char); + + byte rssi_antenna_[] = { + 0b10101, + 0b10001, + 0b01110, + 0b00100, + 0b00100, + 0b00100, + 0b00101, + 0b00101, }; + lcd.createChar(rssiantenna, rssi_antenna_); + + + byte rssi_bars_[8]; + rssi_bars_[0] = 0b00001; + rssi_bars_[1] = 0b00001; + rssi_bars_[2] = 0b00001; + rssi_bars_[3] = 0b00001; + rssi_bars_[4] = 0b00101; + rssi_bars_[5] = 0b00101; + rssi_bars_[6] = 0b10101; + rssi_bars_[7] = 0b10101; + + lcd.createChar(rssi_bars, rssi_bars_); + + + byte battery_char_data[8]; + battery_char_data[0] = 0b01110; + battery_char_data[1] = 0b11011; + battery_char_data[2] = 0b10001; + battery_char_data[3] = 0b10001; + battery_char_data[4] = 0b10001; + battery_char_data[5] = 0b10001; + battery_char_data[6] = 0b10001; + battery_char_data[7] = 0b11111; + lcd.createChar(battery_char, battery_char_data); + + lcd.setCursor(12,0); + lcd.write(battery_char); +#if 0 + lcd.setCursor(12,1); + lcd.write(battery_char); + lcd.setCursor(6,1); + lcd.write(rssiantenna); + lcd.setCursor(7,1); + lcd.write(rssi_bars); +#endif +} + +void LCD_state_fly::print_time(uint16_t time) +{ + char line[17]; + + /** + * 0123456789012345 + * fly mode A PP + * T SSS AA PP A PP + **/ + if(this->last_time == time) + return; + + this->last_time=time; + + lcd.setCursor(1,1); + snprintf(line, sizeof(line), "%*u", 3, time); + lcd.print(line); +} +void LCD_state_fly::print_akku_quad(uint8_t akku_quad) +{ + /** + * 0123456789012345 + * fly mode A PP + * T SSS AA PP A PP + **/ + char line[17]; + + lcd.setCursor(13,1); + snprintf(line, sizeof(line), "%*d", 3, akku_quad); + lcd.print(line); + return; +} + +void LCD_state_fly::print_akku_remote(uint8_t akku_remote) +{ + /** + * 0123456789012345 + * fly mode A PP + * T SSS AA PP A PP + **/ + + char line[17]; + + lcd.setCursor(13,0); + snprintf(line, sizeof(line), "%*d", 3, akku_remote); + lcd.print(line); + return; +} + +void LCD_state_fly::print_rssi(uint8_t rssi_percent) +{ + char line[17]; + + lcd.setCursor(8,1); + snprintf(line, sizeof(line), "%*d", 3, rssi_percent); + lcd.print(line); +} + +void LCD_state_fly::update(void) +{ + uint8_t call=0; + uint8_t rssi_percent = 100; + uint8_t akku_quad = 1; + uint8_t akku_remote = 2; + unsigned long time_in_ms = millis() - this->time_enter; + unsigned long time_in_s = time_in_ms/1000; // to sec + + uint32_t end__ = micros(); + uint32_t start = micros(); + uint32_t next_callback_time; + next_callback_time = initFrSky_2way(); + + input.update(); + // init for bind + frsky2way_init(1); + + input.print(); + while(1) { + start = end__; + next_callback_time = ReadFrSky_2way(); + + //if (next_callback_time > 9000) { + if (next_callback_time == 9200) { + input.update(); + + if (input.is_menu_triggered()) { + debug("%lu menu button trigger\n", millis()); + input.print(); + break; + } + } + + + if (next_callback_time == 9200) { + // print on lcd + call +=2; + if(call > 50) + call= 0; + + switch(call) + { + + + case 10: + // update time + time_in_ms = millis() - this->time_enter; + time_in_s = time_in_ms/1000; // to sec + this->print_time(time_in_s); + break; + +#if 0 + case 20: + rssi_percent += 1; + if(rssi_percent > 100) + rssi_percent = 0; + this->print_rssi(rssi_percent); + break; + + case 30: + // update akku + akku_quad += 1; + if(akku_quad > 100) + akku_quad = 0; + this->print_akku_quad(akku_quad); + break; + +#endif + case 40: + // update akku + akku_remote = analogRead(Battery_pin); + if (akku_remote > 3830) + akku_remote = 3830; + if (akku_remote < 3450) + akku_remote = 3450; + akku_remote = map16b(akku_remote, 3450, 3830, 0, 100); + if (akku_remote > 100) + akku_remote = 100; + this->print_akku_remote(akku_remote); + break; + + default: + break; + } + + } + + + end__ = micros(); + if (end__ - start < next_callback_time) { + uint32_t wait = next_callback_time; + wait -= (end__ - start); + delayMicroseconds(wait); + /* if((end__ - start) > 1000 ) { */ + /* debug("call %d waited %lu timed %lu vs wait %lu \n", call,next_callback_time, wait, (end__ - start)); */ + /* print_frskyd_telemetry(); */ + /* } */ + } + end__ = micros(); + } + + // cange to menu when done + new_state = s_menu; + +} +void LCD_state_fly::leave(void) +{ + lcd.clear(); +} diff --git a/remote/src/state_init.cpp b/remote/src/state_init.cpp new file mode 100644 index 0000000..2530d30 --- /dev/null +++ b/remote/src/state_init.cpp @@ -0,0 +1,58 @@ +#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; + delay(100); + diff = millis() - this->time_enter; + if (diff > 3 * 1000) { + new_state = s_joy; + debugln("read start"); + delay(1000); + if ( 0 > eeprom_config.read()) { + debugln("failed to read"); + return; + } + debugln("read fin"); + + debugln("val start"); + if ( 0 > eeprom_config.validate()) { + debugln("failed to validate"); + return; + } + debugln("val fin"); + + new_state = s_menu; + + struct Input::ch_config ch_config[Input::CH_COUNT]; + eeprom_config.get_ch_config(ch_config); + input.set_calibration(ch_config); + + uint32_t master_id = 0; + eeprom_config.get_master_id(&master_id); + set_rx_tx_addr(master_id); + + + } +} +void LCD_state_init::leave(void) +{ + lcd.clear(); +} diff --git a/remote/src/state_joy_calib.cpp b/remote/src/state_joy_calib.cpp new file mode 100644 index 0000000..a4e4861 --- /dev/null +++ b/remote/src/state_joy_calib.cpp @@ -0,0 +1,134 @@ +#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); + eeprom_config.set_ch_config(ch_config); + uint32_t id=0; + #ifdef FORCE_GLOBAL_ID + id = FORCE_GLOBAL_ID; + #else + // Generate a random ID from UUID + #define STM32_UUID ((uint32_t *)0x1FFFF7E8) + id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; + #endif + debugln("Generated new master ID %lx", id); + eeprom_config.set_master_id(id); + eeprom_config.write(); + + eeprom_config.read(); + if (eeprom_config.validate()) { + debugln("ok calib\n"); + }else { + debugln("failed calib\n"); + } + + + lcd.setCursor(0,0); + lcd.print("center again "); + lcd.setCursor(0,1); + lcd.print("all sticks "); + while (false == input.is_centered()) { + input.update(); + } + new_state = s_menu; +} +void LCD_state_joy_calibration::leave(void) { + lcd.setCursor(0,0); + lcd.print("finished "); + lcd.setCursor(0,1); + lcd.print("calibration"); +} diff --git a/remote/src/state_joy_usb.cpp b/remote/src/state_joy_usb.cpp new file mode 100644 index 0000000..2a10187 --- /dev/null +++ b/remote/src/state_joy_usb.cpp @@ -0,0 +1,64 @@ +#include +#include +#include "Arduino.h" +#include "state.h" +#include "input.h" +#include "debug.h" + +int16_t map16b( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); +#include +extern HIDJoystick Joystick; +LCD_state_joy_usb::LCD_state_joy_usb(void) { +} +void LCD_state_joy_usb::enter(void) { + lcd.setCursor(0,0); + lcd.print("joystick mode "); + lcd.setCursor(0,1); + lcd.print(" "); + delay(500); +} +void LCD_state_joy_usb::update(void) { + input.update(); + + uint16_t *ch_data= input.get_channel_data(); + + uint16_t x = map16b( ch_data[Input::CH_THROTTLE], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); + uint16_t y = map16b( ch_data[Input::CH_YAW], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); + + uint16_t xr = map16b( ch_data[Input::CH_ROLL], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); + uint16_t yr = map16b( ch_data[Input::CH_PITCH], CHANNEL_MIN_100, CHANNEL_MAX_100, 0, 1023); + + bool bt0 = ch_data[Input::CH_AUX1] == CHANNEL_MIN_100; + bool bt1 = ch_data[Input::CH_AUX2] == CHANNEL_MIN_100; + bool bt2 = ch_data[Input::CH_AUX3] == CHANNEL_MIN_100; + bool bt3 = ch_data[Input::CH_AUX4] == CHANNEL_MIN_100; + bool bt4 = ch_data[Input::CH_AUX5] == CHANNEL_MIN_100; + delay(50); + + Joystick.X(x); + Joystick.Y(y); + Joystick.Xrotate(xr); + Joystick.Yrotate(yr); + Joystick.button(0, bt0); + Joystick.button(1, bt1); + Joystick.button(2, bt2); + Joystick.button(3, bt3); + Joystick.button(4, bt4); + + char line[17]; + snprintf(line,sizeof(line),"%lu %lu", bt0 , input.ch_raw[Input::CH_AUX1]); + lcd.setCursor(0,1); + lcd.print(line); + + + if (input.is_menu_triggered()) { + debug("%lu menu button trigger\n", millis); + new_state = s_menu; + } +} +void LCD_state_joy_usb::leave(void) { + lcd.setCursor(0,0); + lcd.print("finished "); + lcd.setCursor(0,1); + lcd.print("usb mode "); +} diff --git a/remote/src/state_menu.cpp b/remote/src/state_menu.cpp new file mode 100644 index 0000000..c3fae82 --- /dev/null +++ b/remote/src/state_menu.cpp @@ -0,0 +1,86 @@ +#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 usb ", s_usb}, + { "Joy calib ", s_joy }, + { "HF calib ", NULL }, + { " ", NULL }, + }; + + bool wait = false; + char curr[2][16]; + snprintf(curr[0], sizeof(curr[0]), "> %s", menus[this->curr_selected].name); + snprintf(curr[1], sizeof(curr[1]), " %s", menus[this->curr_selected+1].name); + + lcd.setCursor(0,0); + lcd.print(curr[0]); + lcd.setCursor(0,1); + lcd.print(curr[1]); + + input.update(); + if (false == input.is_centered(Input::MENU_UP_DOWN)) { + if (input.is_low(Input::MENU_UP_DOWN)){ + this->curr_selected +=1; + if ( this->curr_selected > 3) + this->curr_selected = 3; + else + wait = true; + } + if (input.is_high(Input::MENU_UP_DOWN)){ + this->curr_selected -=1; + if ( this->curr_selected < 0) + this->curr_selected = 0; + else + wait = true; + } + } + + if (input.is_high(Input::MENU_LEFT_RIGHT)) { + lcd.setCursor(0,0); + lcd.print("entering "); + lcd.setCursor(0,1); + lcd.print(menus[this->curr_selected].name); + // do wait until its centered + while(false == input.is_centered(Input::MENU_LEFT_RIGHT)) { + input.update(); + } + + if (menus[this->curr_selected].state != NULL) { + new_state = menus[this->curr_selected].state; + } + wait = true; + } + + if(wait) + delay(500); +} + +void LCD_state_menu::leave(void) +{ + lcd.clear(); +} 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 **/ +/**************************/ +/**************************/