add invert calibration

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

Multiprotocol/Multiprotocol.ino

@@ -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,25 +574,7 @@ 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__);
 }
 

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,6 +26,7 @@ struct Input::data* Input::get_curr_input(void) {
 struct Input::data* Input::get_old_input(void) {
     return this->old;
 }
+
 void Input::init() {
   analogReadResolution(16);
 
@@ -57,6 +57,7 @@ void Input::init() {
   this->yaw.min       = 500;
   this->roll.min      = 500;
   this->pitch.min     = 500;
+
   this->throttle.max  = 500;
   this->yaw.max       = 500;
   this->roll.max      = 500;
@@ -64,8 +65,10 @@ void Input::init() {
 }
 void Input::do_calibration(void) {
   int8_t turns = 50;
-  int8_t i = turns;
+  int8_t i;
 
+  // min max calibration
+  i = turns;
   while(i > 0) {
     this->update();
     if (true == this->save_calibration()) {
@@ -87,6 +90,220 @@ 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) {
@@ -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;
-}

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