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);