From 490eb0003f417cdac1f1571dadb2197f5fc95044 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Wed, 20 Mar 2019 23:20:18 +0100 Subject: [PATCH] add menu structure and state changes --- Multiprotocol/.gitignore | 5 - remote/.gitignore | 4 + remote/include/debug.h | 2 +- remote/include/input.h | 7 +- remote/include/pins.h | 6 +- remote/include/state.h | 10 +- remote/src/Multiprotocol.cpp | 21 ++-- remote/src/input.cpp | 134 ++++++++--------------- remote/src/state.cpp | 207 ++++++++++++++++++++++++++++++++++- 9 files changed, 281 insertions(+), 115 deletions(-) delete mode 100644 Multiprotocol/.gitignore diff --git a/Multiprotocol/.gitignore b/Multiprotocol/.gitignore deleted file mode 100644 index f152028..0000000 --- a/Multiprotocol/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -.pio -.pioenvs -.piolibdeps -.clang_complete -.gcc-flags.json diff --git a/remote/.gitignore b/remote/.gitignore index 6c69f4c..54eefeb 100644 --- a/remote/.gitignore +++ b/remote/.gitignore @@ -1,2 +1,6 @@ +.pio .pioenvs .piolibdeps +.clang_complete +.gcc-flags.json + diff --git a/remote/include/debug.h b/remote/include/debug.h index 2099521..f511621 100644 --- a/remote/include/debug.h +++ b/remote/include/debug.h @@ -8,7 +8,7 @@ //******************** #define ENABLE_DBEUG // ~1k #ifdef ENABLE_DBEUG - #define debug(msg, ...) { char buf[256]; sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} + #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);} #define debug_time(msg) { uint16_t debug_time_TCNT1=TCNT1; debug_time=debug_time_TCNT1-debug_time; debugln(msg "%u", debug_time); debug_time=debug_time_TCNT1; } diff --git a/remote/include/input.h b/remote/include/input.h index 910904d..22484e2 100644 --- a/remote/include/input.h +++ b/remote/include/input.h @@ -7,6 +7,7 @@ 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 { @@ -52,6 +53,10 @@ class Input { // menu inputs bool is_menu_triggered(void); + void invert_ch(enum input_channels ch); + void print_ch(enum input_channels ch); + void calibration_init(void); + bool calibration_update(void); private: struct data input[2]; struct data* curr; @@ -67,8 +72,6 @@ class Input { } ch_config[CH_COUNT]; uint32_t pins[CH_COUNT]; - - bool calibration_update(void); }; extern uint16_t Channel_data[NUM_TX_CHN]; diff --git a/remote/include/pins.h b/remote/include/pins.h index 931cdac..f8c1321 100644 --- a/remote/include/pins.h +++ b/remote/include/pins.h @@ -32,8 +32,10 @@ #define Throttle_pin PA4 #define Yaw_pin PA5 -#define Roll_pin PA6 -#define Pitch_pin PA7 +//#define Roll_pin PA6 +//#define Pitch_pin PA7 +#define Roll_pin PA7 +#define Pitch_pin PA6 #define Aux1_pin PB0 #define Aux2_pin PB0 diff --git a/remote/include/state.h b/remote/include/state.h index 99a631b..c05d4db 100644 --- a/remote/include/state.h +++ b/remote/include/state.h @@ -62,9 +62,7 @@ public: class LCD_state_menu: public State { private: - unsigned long time_enter; - uint8_t curr_selected; - + int8_t curr_selected; public: LCD_state_menu(void); void enter(void); @@ -72,11 +70,9 @@ public: void leave(void); }; -class LCD_state_calibration: public State { -private: - unsigned long time_enter; +class LCD_state_joy_calibration: public State { public: - LCD_state_calibration(void); + LCD_state_joy_calibration(void); void enter(void); void update(void); void leave(void); diff --git a/remote/src/Multiprotocol.cpp b/remote/src/Multiprotocol.cpp index 116e60d..f1b9d6a 100644 --- a/remote/src/Multiprotocol.cpp +++ b/remote/src/Multiprotocol.cpp @@ -223,7 +223,7 @@ void setup() sub_protocol = PPM_prot[line].sub_proto; RX_num = PPM_prot[line].rx_num; - debugln("protocol: %d", protocol); + debug("protocol: %d ", protocol); switch(protocol) { case PROTO_FRSKYD: debugln("PROTO_FRSKYD"); @@ -235,9 +235,9 @@ void setup() debugln("PROTO_FRSKYV"); break; } - debugln("sub_protocol: %d", sub_protocol); + debug("sub_protocol: %d\n", sub_protocol); option = PPM_prot[line].option; // Use radio-defined option value - debugln("freq offset: %d", option); + debug("freq offset: %d\n", option); if(PPM_prot[line].power) POWER_FLAG_on; if(PPM_prot[line].autobind) { @@ -250,20 +250,27 @@ void setup() } #endif //ENABLE_PPM - debugln("Init complete"); + debug("Init complete\n"); input.init(); input.update(); init_state(); - debugln("do calibration start moving sticks please"); - input.do_calibration(); } // Main // Protocol scheduler void loop() { + uint32_t s; + s =micros(); + input.update(); + debug("input took %lu", (micros()-s)); + + s =micros(); + update_state(); + debugln("state took %lu", (micros()-s)); + return; uint32_t next_callback; if(remote_callback==0 || IS_WAIT_BIND_on ) { @@ -284,7 +291,7 @@ void loop() s =micros(); input.update(); - debugln("input took %lu", (micros()-s)); + debug("input took %lu", (micros()-s)); s =micros(); update_state(); diff --git a/remote/src/input.cpp b/remote/src/input.cpp index c54a186..807303f 100644 --- a/remote/src/input.cpp +++ b/remote/src/input.cpp @@ -103,78 +103,12 @@ void Input::init() { this->ch_config[CH_AUX4].inverted = false; this->ch_config[CH_AUX5].inverted = false; - this->ch_config[CH_THROTTLE].min = 0; - this->ch_config[CH_YAW].min = 0; - this->ch_config[CH_ROLL].min = 0; - this->ch_config[CH_PITCH].min = 0; - - this->ch_config[CH_THROTTLE].max = 4096; - this->ch_config[CH_YAW].max = 4096; - this->ch_config[CH_ROLL].max = 4096; - this->ch_config[CH_PITCH].max = 4096; } void Input::do_calibration(void) { - int8_t turns = 50; - int8_t i; - - // min max calibration - i = turns; - while(i > 0) { - this->update(); - if (true == this->calibration_update()) { - i = turns; - debugln("new values t %d-%d r %d-%d p %d-%d y %d-%d", - this->ch_config[CH_THROTTLE].min, this->ch_config[CH_THROTTLE].max, - this->ch_config[CH_ROLL].min, this->ch_config[CH_ROLL].max, - this->ch_config[CH_PITCH].min, this->ch_config[CH_PITCH].max, - this->ch_config[CH_YAW].min, this->ch_config[CH_YAW].max); - }else { - i -= 1; - } - - delay(100); - } - - // center - debugln("now center all sticks"); - i = turns; - while(i > 0) { - delay(100); - this->update(); - if (false == this->is_centered()) { - i = turns; - }else { - i -= 1; - } - - } - - for (int ch = 0; ch < 4 ; ++ch) { - debugln("now Move %s to max", ch_name[ch]); - i = turns; - while(i>0) { - delay(50); - this->update(); - - if (true == this->is_high((enum input_channels)ch)) { - debug("u"); - i--; - continue; - } - - - if (true == this->is_low((enum input_channels)ch)) { - debug("dI"); - this->ch_config[CH_THROTTLE].inverted = !this->ch_config[CH_THROTTLE].inverted; - i = turns; - continue; - } - } - } - } + bool Input::is_centered(void) { - return + return this->is_centered(CH_ROLL) && this->is_centered(CH_PITCH) && this->is_centered(CH_THROTTLE) && @@ -196,8 +130,8 @@ 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 / 3; - if ( this->curr->ch_data[ch] < this->ch_config[ch].max - delta) { + uint16_t delta = range / 5; + if ( this->curr->ch_data[ch] > this->ch_config[ch].max - delta) { return true; } return false; @@ -205,7 +139,7 @@ bool Input::is_high(enum Input::input_channels ch) { } 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 / 3; + uint16_t delta = range / 5; if ( this->curr->ch_data[ch] < this->ch_config[ch].min + delta) { return true; } @@ -216,23 +150,51 @@ bool Input::is_menu_triggered(void) { return this->curr->menu; } -bool Input::calibration_update(void) { - bool changed = false; - - for (uint8_t ch = 0; ch < CH_COUNT; ch++) { - if (this->ch_config[ch].min > this->ch_raw[ch]) { - this->ch_config[ch].min = this->ch_raw[ch]; - changed = true; - } else if (this->ch_config[ch].max < this->ch_raw[ch]) { - changed = true; - this->ch_config[ch].max = this->ch_raw[ch]; - } +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", + ch, + this->ch_raw[ch], + this->curr->ch_data[ch], + this->ch_config[ch].min, + this->ch_config[ch].max, + this->is_high((enum Input::input_channels)ch), + this->is_centered((enum Input::input_channels)ch), + this->is_low((enum Input::input_channels)ch) + ); +} +bool Input::calibration_update(void) { + bool changed = false; + + for (uint8_t ch = 0; ch < CH_COUNT; ch++) { + if (this->ch_config[ch].min > this->ch_raw[ch]) { + this->ch_config[ch].min = this->ch_raw[ch]; + changed = true; + } else if (this->ch_config[ch].max < this->ch_raw[ch]) { + changed = true; + this->ch_config[ch].max = this->ch_raw[ch]; + } + } - } + if (changed) { + debugln("new calib values t %d-%d r %d-%d p %d-%d y %d-%d", + this->ch_config[CH_THROTTLE].min, this->ch_config[CH_THROTTLE].max, + this->ch_config[CH_ROLL].min, this->ch_config[CH_ROLL].max, + this->ch_config[CH_PITCH].min, this->ch_config[CH_PITCH].max, + this->ch_config[CH_YAW].min, this->ch_config[CH_YAW].max); + // TODO save in eeprom + } - // TODO save in eeprom - return changed; + return changed; +} +void Input::calibration_init(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) { @@ -250,7 +212,7 @@ void Input::update(void) { 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]) { diff --git a/remote/src/state.cpp b/remote/src/state.cpp index 7d634cd..eb50493 100644 --- a/remote/src/state.cpp +++ b/remote/src/state.cpp @@ -1,7 +1,9 @@ #include #include #include "Arduino.h" +#include "FrSkyD_cc2500.h" #include "state.h" +#include "input.h" #include "debug.h" #include "tx_def.h" @@ -69,6 +71,7 @@ void init_state(void) { s_init = new LCD_state_init(); s_bind = new LCD_state_bind(); s_fly = new LCD_state_fly(); + s_joy = new LCD_state_joy_calibration(); s_menu = new LCD_state_menu(); lcd.backlight(); curr_state = NULL; @@ -109,7 +112,7 @@ void LCD_state_init::update(void) uint32_t diff; diff = millis() - this->time_enter; if (diff > 5 * 1000) { - new_state = s_bind; + new_state = s_joy; } } void LCD_state_init::leave(void) @@ -140,9 +143,42 @@ void LCD_state_bind::update(void) lcd.setCursor(0,1); lcd.print(line); - // cange to menu when done - if (time_in_s >= this->bind_time) - new_state = s_menu; + 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) @@ -150,6 +186,111 @@ 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) { } @@ -158,13 +299,69 @@ void LCD_state_menu::enter(void) { lcd.print("menu mode "); lcd.setCursor(0,1); lcd.print(" "); - this->time_enter = millis(); + 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();