From 9fc6cdcf741925e96372d05264ebabd8098c8710 Mon Sep 17 00:00:00 2001
From: "Schoenberger, Philipp" <ph.schoenberger@posteo.de>
Date: Wed, 13 Feb 2019 11:04:31 +0100
Subject: [PATCH] add invert calibration

---
 Multiprotocol/FrSkyD_cc2500.h   |   6 +
 Multiprotocol/Multiprotocol.ino |  39 +----
 Multiprotocol/input.cpp         | 264 ++++++++++++++++++++++++++++----
 Multiprotocol/input.h           |  23 ++-
 4 files changed, 264 insertions(+), 68 deletions(-)

diff --git a/Multiprotocol/FrSkyD_cc2500.h b/Multiprotocol/FrSkyD_cc2500.h
index 4af0aa9..da8c7ee 100644
--- a/Multiprotocol/FrSkyD_cc2500.h
+++ b/Multiprotocol/FrSkyD_cc2500.h
@@ -18,8 +18,14 @@
 
 #include <stdint.h>
 void frsky2way_init(uint8_t bind);
+
 void frsky2way_build_bind_packet();
+
 void frsky2way_data_frame(void);
+
 uint16_t initFrSky_2way(void);
 uint16_t ReadFrSky_2way(void);
+
+
+
 #endif
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index d6cd04a..7bb1866 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -288,7 +288,7 @@ void setup()
   input.update();
   init_state();
 
-  
+
   debugln("do calibration start moving sticks please");
   input.do_calibration();
 }
@@ -330,8 +330,7 @@ void loop()
     }
 }
 
-uint8_t Update_All()
-{
+uint8_t Update_All() {
     #ifdef ENABLE_SERIAL
         if(mode_select==MODE_SERIAL && IS_RX_FLAG_on)       // Serial mode and something has been received
         {
@@ -398,8 +397,7 @@ uint8_t Update_All()
 }
 
 // Update channels direction and Channel_AUX flags based on servo AUX positions
-static void update_channels_aux(void)
-{
+static void update_channels_aux(void) {
     //Reverse channels direction
     #ifdef REVERSE_AILERON
         reverse_channel(AILREON);
@@ -422,8 +420,7 @@ static void update_channels_aux(void)
 }
 
 #ifdef ENABLE_PPM
-uint8_t bank_switch(void)
-{
+uint8_t bank_switch(void) {
     uint8_t bank= curr_bank;
     if(bank>=NBR_BANKS)
     { // Wrong number of bank
@@ -531,11 +528,9 @@ inline void tx_resume()
 }
 
 // Protocol start
-static void protocol_init()
-{
+static void protocol_init() {
     static uint16_t next_callback;
-    if(IS_WAIT_BIND_off)
-    {
+    if(IS_WAIT_BIND_off) {
         remote_callback = 0;            // No protocol
         next_callback=0;                // Default is immediate call back
         modules_reset();                // Reset all modules
@@ -579,26 +574,8 @@ static void protocol_init()
     WAIT_BIND_off;
     CHANGE_PROTOCOL_FLAG_off;
 
-#if 0
-    if(next_callback>32000)
-    { // next_callback should not be more than 32767 so we will wait here...
-        uint16_t temp=(next_callback>>10)-2;
-        delayMilliseconds(temp);
-        next_callback-=temp<<10;                // between 2-3ms left at this stage
-    }
-    cli();                                      // disable global int
-    OCR1A = TCNT1 + next_callback*2;            // set compare A for callback
-    #ifndef STM32_BOARD
-        TIFR1 = OCF1A_bm ;                      // clear compare A flag
-    #else
-        TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag
-    #endif
-    sei();                                      // enable global int
-    BIND_BUTTON_FLAG_off;                       // do not bind/reset id anymore even if protocol change
-#else
-  delayMicroseconds(next_callback);
-#endif
-  debugln("%s BIND_BUTTON_FLAG_off",__func__);
+    delayMicroseconds(next_callback);
+    debugln("%s BIND_BUTTON_FLAG_off",__func__);
 }
 
 void update_serial_data()
diff --git a/Multiprotocol/input.cpp b/Multiprotocol/input.cpp
index 3dbb462..06fc134 100644
--- a/Multiprotocol/input.cpp
+++ b/Multiprotocol/input.cpp
@@ -12,7 +12,6 @@ Input::Input(void) {
     this->curr = &(this->input[0]);
     this->old  = &(this->input[1]);
     memset(this->input,0, sizeof(this->input));
-    
 }
 void Input::mark_processed(void) {
     struct data* temp = this->old;
@@ -27,9 +26,10 @@ struct Input::data* Input::get_curr_input(void) {
 struct Input::data* Input::get_old_input(void) {
     return this->old;
 }
+
 void Input::init() {
   analogReadResolution(16);
-  
+
   pinMode(Throttle_pin,INPUT);
   pinMode(Yaw_pin,INPUT);
   pinMode(Roll_pin,INPUT);
@@ -52,20 +52,23 @@ void Input::init() {
   this->aux[4].inverted   = false;
   this->aux[5].inverted   = false;
 
-  
+
   this->throttle.min  = 500;
   this->yaw.min       = 500;
   this->roll.min      = 500;
   this->pitch.min     = 500;
+
   this->throttle.max  = 500;
   this->yaw.max       = 500;
   this->roll.max      = 500;
   this->pitch.max     = 500;
 }
-void Input::do_calibration(void) { 
-  int8_t turns = 50; 
-  int8_t i = turns;
-  
+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->save_calibration()) {
@@ -78,7 +81,7 @@ void Input::do_calibration(void) {
     }else {
       i -= 1;
     }
-            
+
     delay(100);
   }
   debugln("end values t %d-%d r %d-%d p %d-%d y %d-%d",
@@ -87,8 +90,222 @@ void Input::do_calibration(void) {
           this->pitch.min,   this->pitch.max,
           this->yaw.min,     this->yaw.max);
 
+  // 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;
+    }
+
+  }
+
+  debugln("now Move throttle to max");
+  i = turns;
+  while(i>0) {
+    delay(100);
+    this->update();
+
+    if (true == this->is_throttle_up()) {
+        i--;
+        continue;
+    }
+    if (true == this->is_throttle_down()) {
+        this->throttle.inverted = ! this->throttle.inverted;
+        i = turns;
+        continue;
+    }
+  }
+  debugln("throttle invert is %d", this->throttle.inverted);
+
+
+  debugln("now Move yaw to max");
+  i = turns;
+  while(i>0) {
+    delay(100);
+    this->update();
+
+    if (true == this->is_yaw_up()) {
+        i--;
+        continue;
+    }
+    if (true == this->is_yaw_down()) {
+        this->yaw.inverted = ! this->yaw.inverted;
+        i = turns;
+        continue;
+    }
+  }
+  debugln("yaw invert is %d", this->yaw.inverted);
+
+  debugln("now Move pitch to max");
+  i = turns;
+  while(i>0) {
+    delay(100);
+    this->update();
+
+    if (true == this->is_pitch_up()) {
+        i--;
+        continue;
+    }
+    if (true == this->is_pitch_down()) {
+        this->pitch.inverted = ! this->pitch.inverted;
+        i = turns;
+        continue;
+    }
+  }
+  debugln("pitch invert is %d", this->pitch.inverted);
+
+  debugln("now Move roll to max");
+  i = turns;
+  while(i>0) {
+    delay(100);
+    this->update();
+
+    if (true == this->is_roll_up()) {
+        i--;
+        continue;
+    }
+    if (true == this->is_roll_down()) {
+        this->roll.inverted = ! this->roll.inverted;
+        i = turns;
+        continue;
+    }
+  }
+  debugln("roll invert is %d", this->roll.inverted);
+
+}
+bool Input::is_centered(void) {
+    return this->is_centered_left() && this->is_centered_right();
+}
+
+bool Input::is_centered_left(void) {
+    uint16_t range;
+    uint16_t delta;
+
+    range = this->throttle.max - this->throttle.min;
+    delta = range >>4;
+    if ( this->curr->throttle < this->throttle.min + range/2 - delta ||
+         this->curr->throttle > this->throttle.min + range/2 + delta
+    ) {
+        // throttle is not centered
+        return false;
+    }
+
+
+    range = this->yaw.max - this->yaw.min;
+    delta = range >>4;
+    if ( this->curr->yaw < this->yaw.min + range/2 - delta ||
+         this->curr->yaw > this->yaw.min + range/2 + delta
+    ) {
+        // yaw is not centered
+        return false;
+    }
+    return true;
 }
-  
+bool Input::is_centered_right(void) {
+    uint16_t range;
+    uint16_t delta;
+
+    range = this->pitch.max - this->pitch.min;
+    delta = range >>4;
+    if ( this->curr->pitch < this->pitch.min + range/2 - delta ||
+         this->curr->pitch > this->pitch.min + range/2 + delta
+    ) {
+        // pitch is not centered
+        return false;
+    }
+
+    range = this->roll.max - this->roll.min;
+    delta = range >>4;
+    if ( this->curr->roll < this->roll.min + range/2 - delta ||
+         this->curr->roll > this->roll.min + range/2 + delta
+    ) {
+        // roll is not centered
+        return false;
+    }
+    return true;
+}
+
+bool Input::is_menu_left(void) {
+    return this->is_roll_down();
+}
+bool Input::is_menu_right(void) {
+    return this->is_roll_up();
+}
+bool Input::is_menu_down(void) {
+    return this->is_pitch_down();
+}
+bool Input::is_menu_up(void) {
+    return this->is_pitch_up();
+}
+bool Input::is_throttle_down(void) {
+    uint16_t delta = (this->throttle.max - this->throttle.min)/3;
+    if ( this->curr->throttle < this->throttle.min + delta) {
+        return true;
+    }
+    return false;
+
+}
+bool Input::is_throttle_up(void) {
+    uint16_t delta = (this->throttle.max - this->throttle.min)/3;
+    if ( this->curr->throttle > this->throttle.max - delta) {
+        return true;
+    }
+    return false;
+}
+bool Input::is_yaw_up(void) {
+    uint16_t delta = (this->yaw.max - this->yaw.min)/3;
+    if ( this->curr->yaw > this->yaw.max - delta) {
+        return true;
+    }
+    return false;
+}
+bool Input::is_yaw_down(void) {
+    uint16_t delta = (this->yaw.max - this->yaw.min)/3;
+    if ( this->curr->yaw > this->yaw.min + delta) {
+        return true;
+    }
+    return false;
+}
+
+bool Input::is_roll_down(void) {
+    uint16_t delta = (this->roll.max - this->roll.min)/3;
+    if ( this->curr->roll < this->roll.min + delta) {
+        return true;
+    }
+    return false;
+
+}
+bool Input::is_roll_up(void) {
+    uint16_t delta = (this->roll.max - this->roll.min)/3;
+    if ( this->curr->roll > this->roll.max - delta) {
+        return true;
+    }
+    return false;
+}
+bool Input::is_pitch_up(void) {
+    uint16_t delta = (this->pitch.max - this->pitch.min)/3;
+    if ( this->curr->pitch > this->pitch.max - delta) {
+        return true;
+    }
+    return false;
+}
+bool Input::is_pitch_down(void) {
+    uint16_t delta = (this->pitch.max - this->pitch.min)/3;
+    if ( this->curr->pitch > this->pitch.min + delta) {
+        return true;
+    }
+    return false;
+}
+
+bool Input::is_menu_triggered(void) {
+    return this->curr->menu;
+}
+
 bool Input::save_calibration(void) {
   bool changed = false;
   if (this->throttle.min > this->curr->throttle){
@@ -107,7 +324,7 @@ bool Input::save_calibration(void) {
     this->yaw.max = this->curr->yaw;
   }
 
-  
+
   if (this->roll.min > this->curr->roll){
     changed = true;
     this->roll.min = this->curr->roll;
@@ -115,7 +332,7 @@ bool Input::save_calibration(void) {
     changed = true;
     this->roll.max = this->curr->roll;
   }
-  
+
   if (this->pitch.min > this->curr->pitch){
     changed = true;
     this->pitch.min = this->curr->pitch;
@@ -140,7 +357,7 @@ void Input::update(void) {
 
     this->curr->menu = digitalRead(Menu_pin);
 
-    
+
     /*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],
@@ -156,18 +373,18 @@ void Input::update(void) {
     else
       Channel_data[THROTTLE] = map(this->curr->throttle, this->throttle.min, this->throttle.max, CHANNEL_MIN_100, CHANNEL_MAX_100);
 
-      
+
     if(this->yaw.inverted)
       Channel_data[RUDDER] = map(this->curr->yaw, this->yaw.min, this->yaw.max, CHANNEL_MAX_100, CHANNEL_MIN_100);
     else
       Channel_data[RUDDER] = map(this->curr->yaw, this->yaw.min, this->yaw.max, CHANNEL_MIN_100, CHANNEL_MAX_100);
-      
+
     if(this->roll.inverted)
       Channel_data[AILERON] = map(this->curr->roll, this->roll.min, this->roll.max, CHANNEL_MAX_100, CHANNEL_MIN_100);
     else
       Channel_data[AILERON] = map(this->curr->roll, this->roll.min, this->roll.max, CHANNEL_MIN_100, CHANNEL_MAX_100);
 
-      
+
     if(this->pitch.inverted)
       Channel_data[ELEVATOR] = map(this->curr->pitch, this->pitch.min, this->pitch.max, CHANNEL_MAX_100, CHANNEL_MIN_100);
     else
@@ -180,20 +397,3 @@ void Input::update(void) {
         Channel_data[CH5+i] = (this->curr->aux[i]) ? CHANNEL_MAX_100 : CHANNEL_MIN_100;
     }
 }
-
-bool Input::menu_triggered(void) {
-    return false;
-}
-
-bool Input::left_triggered(void) {
-    return false;
-}
-bool Input::right_triggered(void) {
-    return false;
-}
-bool Input::down_triggered(void) {
-    return false;
-}
-bool Input::up_triggered(void) {
-    return false;
-}
diff --git a/Multiprotocol/input.h b/Multiprotocol/input.h
index e6935e6..43787f1 100644
--- a/Multiprotocol/input.h
+++ b/Multiprotocol/input.h
@@ -39,13 +39,26 @@ class Input {
         void update_inputs(void);
         void mark_processed(void);
 
+        bool is_centered(void);
+        bool is_centered_left(void);
+        bool is_centered_right(void);
         // menu inputs
-        bool menu_triggered(void);
+        bool is_menu_triggered(void);
 
-        bool left_triggered(void);
-        bool right_triggered(void);
-        bool down_triggered(void);
-        bool up_triggered(void);
+        bool is_menu_left(void);
+        bool is_menu_right(void);
+        bool is_menu_down(void);
+        bool is_menu_up(void);
+
+        bool is_throttle_up(void);
+        bool is_throttle_down(void);
+        bool is_yaw_up(void);
+        bool is_yaw_down(void);
+
+        bool is_roll_up(void);
+        bool is_roll_down(void);
+        bool is_pitch_up(void);
+        bool is_pitch_down(void);
 };
 
 extern uint16_t Channel_data[NUM_CHN];