phschoen
/
diy-copter-remote
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add calibration of inputs

master
Schoenberger, Philipp 6 years ago
parent
87a7f65481
commit
f625675312
4 changed files with 154 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 34
      Multiprotocol/Multiprotocol.ino
  2. 8
      Multiprotocol/debug.h
  3. 138
      Multiprotocol/input.cpp
  4. 13
      Multiprotocol/input.h

34
Multiprotocol/Multiprotocol.ino
View File

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

8
Multiprotocol/debug.h
View File

@ -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

138
Multiprotocol/input.cpp
View File

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

13
Multiprotocol/input.h
View File

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

Write Preview
Loading…
Cancel
Save