add menu structure and state changes

Multiprotocol/.gitignore

@@ -1,5 +0,0 @@
-.pio
-.pioenvs
-.piolibdeps
-.clang_complete
-.gcc-flags.json

remote/.gitignore

@@ -1,2 +1,6 @@
+.pio
 .pioenvs
 .piolibdeps
+.clang_complete
+.gcc-flags.json
+

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

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

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

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

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

remote/src/input.cpp

@@ -103,76 +103,10 @@ 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
     this->is_centered(CH_ROLL) &&
@@ -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,6 +150,22 @@ bool Input::is_menu_triggered(void) {
     return this->curr->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",
+            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;
 
@@ -227,13 +177,25 @@ bool Input::calibration_update(void) {
             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
+    }
+
     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) {
 
   for (uint8_t ch = 0; ch < CH_MAX; ch ++) {

remote/src/state.cpp

@@ -1,7 +1,9 @@
 #include <LiquidCrystal_I2C.h>
 #include <stdio.h>
 #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,8 +143,41 @@ void LCD_state_bind::update(void)
   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
-  if (time_in_s >= this->bind_time)
     new_state = s_menu;
 }
 
@@ -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();