From f625675312c573f444f9552e9748d837b0580aa6 Mon Sep 17 00:00:00 2001 From: "Schoenberger, Philipp" Date: Tue, 12 Feb 2019 21:12:51 +0100 Subject: [PATCH] add calibration of inputs --- Multiprotocol/Multiprotocol.ino | 34 ++------ Multiprotocol/debug.h | 8 +- Multiprotocol/input.cpp | 138 ++++++++++++++++++++++++++++++-- Multiprotocol/input.h | 13 ++- 4 files changed, 154 insertions(+), 39 deletions(-) diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 4cc6c35..d6cd04a 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -152,19 +152,7 @@ void setup() // outputs SDI_output; SCLK_output; - #ifdef A7105_CSN_pin - A7105_CSN_output; - #endif - #ifdef CC25_CSN_pin - CC25_CSN_output; - #endif - #ifdef CYRF_CSN_pin - CYRF_RST_output; - CYRF_CSN_output; - #endif - #ifdef NRF_CSN_pin - NRF_CSN_output; - #endif + CC25_CSN_output; // Timer1 config #ifdef TCCR1A @@ -173,20 +161,7 @@ void setup() #endif // Random //random_init(); - - // Set Chip selects - #ifdef A7105_CSN_pin - A7105_CSN_on; - #endif - #ifdef CC25_CSN_pin CC25_CSN_on; - #endif - #ifdef CYRF_CSN_pin - CYRF_CSN_on; - #endif - #ifdef NRF_CSN_pin - NRF_CSN_on; - #endif // Set SPI lines #ifdef STM32_BOARD initSPI2(); @@ -309,8 +284,13 @@ void setup() else #endif //ENABLE_PPM debugln("Init complete"); + input.init(); input.update(); init_state(); + + + debugln("do calibration start moving sticks please"); + input.do_calibration(); } // Main @@ -334,7 +314,7 @@ void loop() if (next_callback > 4000) { input.update(); - update_state(); + //update_state(); } uint32_t wait_until = start + next_callback; diff --git a/Multiprotocol/debug.h b/Multiprotocol/debug.h index 9396522..0b069f1 100644 --- a/Multiprotocol/debug.h +++ b/Multiprotocol/debug.h @@ -6,7 +6,11 @@ //******************** //** Debug messages ** //******************** - #define debug(msg, ...) { char buf[128]; sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} - #define debugln(msg, ...) { char buf[128]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} + #define debug(msg, ...) { char buf[256]; sprintf(buf, msg, ##__VA_ARGS__); Serial.println(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; } + + #define debugln_input(msg, ...) { char buf[256]; sprintf(buf, "input:" msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} + #define debug_input(msg, ...) { char buf[256]; sprintf(buf, "input:" msg , ##__VA_ARGS__); Serial.println(buf);} + #endif diff --git a/Multiprotocol/input.cpp b/Multiprotocol/input.cpp index 24445dc..3dbb462 100644 --- a/Multiprotocol/input.cpp +++ b/Multiprotocol/input.cpp @@ -12,6 +12,7 @@ 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; @@ -26,7 +27,104 @@ 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); + pinMode(Aux1_pin,INPUT); + pinMode(Aux2_pin,INPUT); + pinMode(Aux3_pin,INPUT); + pinMode(Aux4_pin,INPUT); + pinMode(Aux5_pin,INPUT); + pinMode(Aux6_pin,INPUT); + pinMode(Menu_pin,INPUT); + this->throttle.inverted = true; + this->yaw.inverted = false; + this->roll.inverted = false; + this->pitch.inverted = false; + this->aux[0].inverted = false; + this->aux[1].inverted = false; + this->aux[2].inverted = false; + this->aux[3].inverted = false; + 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; + + while(i > 0) { + this->update(); + if (true == this->save_calibration()) { + i = turns; + debugln("new values t %d-%d r %d-%d p %d-%d y %d-%d", + this->throttle.min,this->throttle.max, + this->roll.min, this->roll.max, + this->pitch.min, this->pitch.max, + this->yaw.min, this->yaw.max); + }else { + i -= 1; + } + + delay(100); + } + debugln("end values t %d-%d r %d-%d p %d-%d y %d-%d", + this->throttle.min,this->throttle.max, + this->roll.min, this->roll.max, + this->pitch.min, this->pitch.max, + this->yaw.min, this->yaw.max); + +} + +bool Input::save_calibration(void) { + bool changed = false; + if (this->throttle.min > this->curr->throttle){ + changed = true; + this->throttle.min = this->curr->throttle; + } else if (this->throttle.max < this->curr->throttle) { + changed = true; + this->throttle.max = this->curr->throttle; + } + + if (this->yaw.min > this->curr->yaw){ + changed = true; + this->yaw.min = this->curr->yaw; + } else if (this->yaw.max < this->curr->yaw) { + changed = true; + this->yaw.max = this->curr->yaw; + } + + + if (this->roll.min > this->curr->roll){ + changed = true; + this->roll.min = this->curr->roll; + } else if (this->roll.max < this->curr->roll) { + changed = true; + this->roll.max = this->curr->roll; + } + + if (this->pitch.min > this->curr->pitch){ + changed = true; + this->pitch.min = this->curr->pitch; + } else if (this->roll.max < this->curr->pitch) { + changed = true; + this->pitch.max = this->curr->pitch; + } + return changed; +} void Input::update(void) { this->curr->throttle = analogRead(Throttle_pin); this->curr->yaw = analogRead(Yaw_pin); @@ -42,20 +140,44 @@ 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], + this->curr->aux[4],this->curr->aux[5],this->curr->menu + );*/ + // only do channeloutpu if needed if (curr_state != s_fly) return; - Channel_data[THROTTLE] = map(this->curr->throttle, 0, 3500, CHANNEL_MIN_100, CHANNEL_MAX_100); - Channel_data[RUDDER] = map(this->curr->yaw, 0, 3500, CHANNEL_MIN_100, CHANNEL_MAX_100); - Channel_data[AILERON] = map(this->curr->roll, 0, 3500, CHANNEL_MIN_100, CHANNEL_MAX_100); - Channel_data[ELEVATOR] = map(this->curr->pitch, 0, 3500, CHANNEL_MIN_100, CHANNEL_MAX_100); + if(this->throttle.inverted) + Channel_data[THROTTLE] = map(this->curr->throttle, this->throttle.min, this->throttle.max, CHANNEL_MAX_100, CHANNEL_MIN_100); + 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 + Channel_data[ELEVATOR] = map(this->curr->pitch, this->pitch.min, this->pitch.max, CHANNEL_MIN_100, CHANNEL_MAX_100); for (uint8_t i = 0; i<6; ++i) { - if(this->curr->aux[i]) - Channel_data[CH5+i] = CHANNEL_MAX_100; - else - Channel_data[CH5+i] = CHANNEL_MIN_100; + if(this->aux[i].inverted) + Channel_data[CH5+i] = (this->curr->aux[i]) ? CHANNEL_MIN_100 : CHANNEL_MAX_100; + else + Channel_data[CH5+i] = (this->curr->aux[i]) ? CHANNEL_MAX_100 : CHANNEL_MIN_100; } } diff --git a/Multiprotocol/input.h b/Multiprotocol/input.h index 7034381..e6935e6 100644 --- a/Multiprotocol/input.h +++ b/Multiprotocol/input.h @@ -16,13 +16,22 @@ class Input { bool menu; }; - int curr_input; - public: struct data input[2]; struct data* curr; struct data* old; + struct { + uint16_t max; + uint16_t min; + uint8_t inverted; + } throttle, yaw, roll, pitch, aux[5]; + + bool save_calibration(void); + public: + Input(void); + void init(void); + void do_calibration(void); void update(void); struct data* get_curr_input(void);