You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
399 lines
10 KiB
399 lines
10 KiB
#include "Arduino.h"
|
|
#include <string.h>
|
|
#include "config.h"
|
|
#include "tx_def.h"
|
|
#include "Multiprotocol.h"
|
|
#include "input.h"
|
|
#include "pins.h"
|
|
#include "state.h"
|
|
Input input;
|
|
|
|
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;
|
|
this->old = this->curr;
|
|
this->curr = temp;
|
|
}
|
|
|
|
struct Input::data* Input::get_curr_input(void) {
|
|
return this->curr;
|
|
}
|
|
|
|
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;
|
|
|
|
// min max calibration
|
|
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);
|
|
|
|
// 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){
|
|
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);
|
|
this->curr->roll = analogRead(Roll_pin);
|
|
this->curr->pitch = analogRead(Pitch_pin);
|
|
|
|
this->curr->aux[0] = digitalRead(Aux1_pin);
|
|
this->curr->aux[1] = digitalRead(Aux2_pin);
|
|
this->curr->aux[2] = digitalRead(Aux3_pin);
|
|
this->curr->aux[3] = digitalRead(Aux4_pin);
|
|
this->curr->aux[4] = digitalRead(Aux5_pin);
|
|
this->curr->aux[5] = digitalRead(Aux6_pin);
|
|
|
|
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;
|
|
|
|
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->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;
|
|
}
|
|
}
|