Code cleanup
This commit is contained in:
parent
a48d4a8551
commit
3f0f19cfde
@ -150,12 +150,12 @@ bool button_led_2_on = false;
|
||||
int button_led_2_mode = 0;
|
||||
|
||||
unsigned long current_timestamp = 0;
|
||||
unsigned long button_timestamp = 0;
|
||||
unsigned long joystick_timestamp = 0;
|
||||
unsigned long led_timestamp = 0;
|
||||
unsigned long indicator_timestamp = 0;
|
||||
unsigned long send_usb_timestamp = 0;
|
||||
|
||||
unsigned long current_timestamp_micros = 0;
|
||||
unsigned long elrs_timestamp_micros = 0;
|
||||
unsigned long send_elrs_timestamp_micros = 0;
|
||||
|
||||
const int BUTTON_FLU = 4; // Front left upper button
|
||||
const int BUTTON_FLD = 5; // Front left lower button
|
||||
@ -166,9 +166,9 @@ const int BUTTON_TRU = 10; // Top upper right button
|
||||
const int BUTTON_TLD = 8; // Top left lower button
|
||||
const int BUTTON_TRD = 11; // Top right lower button
|
||||
|
||||
bool toggle_button_10 = false;
|
||||
bool toggle_button_11 = false;
|
||||
int button_10_previous_value = HIGH;
|
||||
bool toggle_button_arm = false;
|
||||
bool toggle_button_mode = false;
|
||||
int button_arm_previous_value = HIGH;
|
||||
int button_11_previous_value = HIGH;
|
||||
|
||||
#define HID_AXIS_MAX 1023
|
||||
@ -180,14 +180,6 @@ int button_11_previous_value = HIGH;
|
||||
#define DEADZONE_X 50
|
||||
#define DEADZONE_Y 50
|
||||
|
||||
// USB HID data variables
|
||||
int xy_x1 = HID_AXIS_CENTER;
|
||||
int xy_y1 = HID_AXIS_CENTER;
|
||||
int xy_x2 = HID_AXIS_CENTER;
|
||||
int xy_y2 = HID_AXIS_CENTER;
|
||||
int xy_x3 = HID_AXIS_CENTER;
|
||||
int xy_y3 = HID_AXIS_CENTER;
|
||||
|
||||
enum EEPROM_ADR
|
||||
{
|
||||
MAX_X1_ADR_HIGH,
|
||||
@ -219,21 +211,27 @@ enum EEPROM_ADR
|
||||
|
||||
int joystick_counter = 0;
|
||||
int joystick_x1 = 0;
|
||||
int joystick_x1_hid = 0;
|
||||
int joystick_x1_raw = 0;
|
||||
int joystick_x1_max = 4096;
|
||||
int joystick_x1_min = 0;
|
||||
int joystick_x1_center = joystick_x1_max/2;
|
||||
int joystick_y1 = 0;
|
||||
int joystick_y1_hid = 0;
|
||||
int joystick_y1_hid_rev = 0;
|
||||
int joystick_y1_hid_rev_mode = 0;
|
||||
int joystick_y1_raw = 0;
|
||||
int joystick_y1_max = 4096;
|
||||
int joystick_y1_min = 0;
|
||||
int joystick_y1_center = joystick_y1_max/2;
|
||||
int joystick_x2 = 0;
|
||||
int joystick_x2_hid = 0;
|
||||
int joystick_x2_raw = 0;
|
||||
int joystick_x2_max = 4096;
|
||||
int joystick_x2_min = 0;
|
||||
int joystick_x2_center = joystick_x2_max/2;
|
||||
int joystick_y2 = 0;
|
||||
int joystick_y2_hid = 0;
|
||||
int joystick_y2_raw = 0;
|
||||
int joystick_y2_max = 4096;
|
||||
int joystick_y2_min = 0;
|
||||
@ -241,6 +239,8 @@ int joystick_y2_center = joystick_y2_max/2;
|
||||
|
||||
float exp_constant = 0.2;
|
||||
|
||||
int fn_mode = 0;
|
||||
|
||||
#define CALIBRATION_OFF 0
|
||||
#define CALIBRATION_INIT 1
|
||||
#define CALIBRATION_CENTER 2
|
||||
@ -309,194 +309,6 @@ void load_from_eeprom(){
|
||||
joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW);
|
||||
}
|
||||
|
||||
void process_io_data(){
|
||||
|
||||
// Check for calibration mode
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
if (digitalRead(BUTTON_TLD) == HIGH){
|
||||
joystick_calibration_mode = CALIBRATION_CENTER;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
button_led_1_mode = 2;
|
||||
button_led_2_mode = 0;
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_MINMAX;
|
||||
button_led_1_mode = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (joystick_calibration_mode == CALIBRATION_MINMAX){
|
||||
button_led_1_mode = 0;
|
||||
button_led_2_mode = 2;
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_OFF;
|
||||
save_to_eeprom();
|
||||
button_led_2_mode = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Check fn mode
|
||||
int fn_mode = 0;
|
||||
if (digitalRead(BUTTON_TLU) == LOW){
|
||||
fn_mode = 1;
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
fn_mode = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// Check toggle buttons
|
||||
if (fn_mode == 1){
|
||||
if (digitalRead(BUTTON_TLD) != button_10_previous_value){
|
||||
button_10_previous_value = digitalRead(BUTTON_TLD);
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
if (toggle_button_10 == false){
|
||||
toggle_button_10 = true;
|
||||
button_led_1_mode = 1;
|
||||
}
|
||||
else{
|
||||
toggle_button_10 = false;
|
||||
button_led_1_mode = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) != button_11_previous_value){
|
||||
button_11_previous_value = digitalRead(BUTTON_TRD);
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
if (toggle_button_11 == false){
|
||||
toggle_button_11 = true;
|
||||
button_led_2_mode = 1;
|
||||
}
|
||||
else{
|
||||
toggle_button_11 = false;
|
||||
button_led_2_mode = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Set joystick buttons to released
|
||||
for (int i = 1; i < 32; i++){
|
||||
Joystick.button(i, 0);
|
||||
}
|
||||
|
||||
// Set joystick hat to center position
|
||||
Joystick.hat(-1);
|
||||
|
||||
// Set joystick buttons to pressed
|
||||
if (fn_mode == 2){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(12, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(13, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(14, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(15, 1);
|
||||
}
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(8, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(9, 1);
|
||||
}
|
||||
}
|
||||
else{
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(2, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(4, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
Joystick.button(5, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(6, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(7, 1);
|
||||
}
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
Joystick.button(1, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
Joystick.button(3, 1);
|
||||
}
|
||||
if (toggle_button_10){
|
||||
Joystick.button(10, 1);
|
||||
}
|
||||
if (toggle_button_11){
|
||||
Joystick.button(11, 1);
|
||||
}
|
||||
|
||||
// Set axis values
|
||||
Joystick.X(joystick_x1);
|
||||
Joystick.Y(joystick_y1);
|
||||
|
||||
if (fn_mode == 2){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(HID_AXIS_CENTER);
|
||||
Joystick.sliderLeft(joystick_x2);
|
||||
Joystick.sliderRight(joystick_y2);
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(joystick_y2);
|
||||
Joystick.sliderLeft(joystick_x2);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
Joystick.Z(joystick_x2);
|
||||
Joystick.Zrotate(joystick_y2);
|
||||
Joystick.sliderLeft(HID_AXIS_CENTER);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
|
||||
// Set ELRS analog channels
|
||||
elrs_channels[0] = map(joystick_x1, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[1] = map(joystick_y1, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[2] = map(joystick_x2, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[3] = map(joystick_y2, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
|
||||
// Set ELRS digital channels
|
||||
for (int i = 4; i < CRSF_MAX_CHANNEL; i++){
|
||||
elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN;
|
||||
}
|
||||
if (toggle_button_10){
|
||||
elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (toggle_button_11){
|
||||
elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
elrs_channels[9] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
elrs_channels[10] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
void read_io_data(){
|
||||
|
||||
if(joystick_counter == 0){
|
||||
@ -529,14 +341,11 @@ void read_io_data(){
|
||||
}
|
||||
|
||||
// Calculate Y1 joystick values
|
||||
if(joystick_y1_raw > (joystick_y1_center + DEADZONE_Y)){
|
||||
joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_center + DEADZONE_Y), joystick_y1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(joystick_y1_raw < (joystick_y1_center - DEADZONE_Y)){
|
||||
joystick_y1 = constrain(map(joystick_y1_raw, joystick_y1_min, (joystick_y1_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
if(joystick_y1_raw > (joystick_y1_min + DEADZONE_Y)){
|
||||
joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_min + DEADZONE_Y), joystick_y1_max, AXIS_MIN, AXIS_MAX), AXIS_MIN, AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y1 = AXIS_CENTER;
|
||||
joystick_y1 = AXIS_MIN;
|
||||
}
|
||||
|
||||
// Calculate X2 joystick values
|
||||
@ -565,56 +374,90 @@ void read_io_data(){
|
||||
if (joystick_x1 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x1 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x1 = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x1 = constrain(joystick_x1, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
joystick_x1_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x1_hid = constrain(joystick_x1_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x1 = HID_AXIS_CENTER;
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y1 values after applying exp curve
|
||||
if (joystick_y1 != AXIS_CENTER){
|
||||
float joystick_y_float = joystick_y1 / float(AXIS_MAX);
|
||||
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
|
||||
joystick_y1 = int(joystick_y_exp * float(HID_AXIS_MAX));
|
||||
joystick_y1 = constrain(joystick_y1, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y1 = HID_AXIS_CENTER;
|
||||
}
|
||||
// Calculate new Y1 values
|
||||
joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_CENTER, HID_AXIS_MAX);
|
||||
joystick_y1_hid_rev = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_CENTER, HID_AXIS_MIN);
|
||||
|
||||
// Calculate new X2 values after applying exp curve
|
||||
if (joystick_x2 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x2 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x2 = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x2 = constrain(joystick_x2, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
joystick_x2_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x2_hid = constrain(joystick_x2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x2 = HID_AXIS_CENTER;
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y2 values after applying exp curve
|
||||
if (joystick_y2 != AXIS_CENTER){
|
||||
float joystick_y_float = joystick_y2 / float(AXIS_MAX);
|
||||
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
|
||||
joystick_y2 = int(joystick_y_exp * float(HID_AXIS_MAX));
|
||||
joystick_y2 = constrain(joystick_y2, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
joystick_y2_hid = int(joystick_y_exp * float(HID_AXIS_MAX));
|
||||
joystick_y2_hid = constrain(joystick_y2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y2 = HID_AXIS_CENTER;
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Check fn mode
|
||||
fn_mode = 0;
|
||||
if (digitalRead(BUTTON_TLU) == LOW){
|
||||
fn_mode = 1;
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
fn_mode = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// Check toggle buttons
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
toggle_button_mode = true;
|
||||
}
|
||||
else if (digitalRead(BUTTON_FLD) == LOW){
|
||||
toggle_button_mode = false;
|
||||
}
|
||||
|
||||
if (fn_mode == 2){
|
||||
if (digitalRead(BUTTON_TRD) != button_arm_previous_value){
|
||||
button_arm_previous_value = digitalRead(BUTTON_TRD);
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
if (toggle_button_arm == false){
|
||||
toggle_button_arm = true;
|
||||
}
|
||||
else{
|
||||
toggle_button_arm = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
// Calibration mode.
|
||||
joystick_x1 = HID_AXIS_CENTER;
|
||||
joystick_y1 = HID_AXIS_CENTER;
|
||||
joystick_x2 = HID_AXIS_CENTER;
|
||||
joystick_y2 = HID_AXIS_CENTER;
|
||||
joystick_x1 = AXIS_CENTER;
|
||||
joystick_y1 = AXIS_CENTER;
|
||||
joystick_x2 = AXIS_CENTER;
|
||||
joystick_y2 = AXIS_CENTER;
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
joystick_y1_hid = HID_AXIS_CENTER;
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
|
||||
// Check for calibration mode
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
if (digitalRead(BUTTON_TLD) == HIGH){
|
||||
joystick_calibration_mode = CALIBRATION_CENTER;
|
||||
}
|
||||
}
|
||||
// Calibrate joystick center values
|
||||
if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
else if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
joystick_x1_center = joystick_x1_raw;
|
||||
joystick_y1_center = joystick_y1_raw;
|
||||
joystick_x1_max = joystick_x1_center;
|
||||
@ -628,6 +471,10 @@ void read_io_data(){
|
||||
joystick_x2_min = joystick_x2_center;
|
||||
joystick_y2_max = joystick_y2_center;
|
||||
joystick_y2_min = joystick_y2_center;
|
||||
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_MINMAX;
|
||||
}
|
||||
}
|
||||
// Calibrate joystick min/max values
|
||||
else if (joystick_calibration_mode == CALIBRATION_MINMAX){
|
||||
@ -656,6 +503,11 @@ void read_io_data(){
|
||||
if(joystick_y2_raw < joystick_y2_min){
|
||||
joystick_y2_min = joystick_y2_raw;
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_OFF;
|
||||
save_to_eeprom();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -751,6 +603,8 @@ void setup()
|
||||
|
||||
// Check if bootloader mode is enabled
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
digitalWrite(BUTTON_LED_2, HIGH);
|
||||
delay(200);
|
||||
_reboot_Teensyduino_();
|
||||
}
|
||||
|
||||
@ -768,13 +622,146 @@ void loop()
|
||||
/* Read io value and process as fast as possible */
|
||||
read_io_data();
|
||||
|
||||
/* Send data every 1500us */
|
||||
if (current_timestamp_micros >= elrs_timestamp_micros)
|
||||
if (current_timestamp >= send_usb_timestamp)
|
||||
{
|
||||
process_io_data();
|
||||
// Set joystick buttons to released
|
||||
for (int i = 1; i < 32; i++){
|
||||
Joystick.button(i, 0);
|
||||
}
|
||||
|
||||
// Set joystick hat to center position
|
||||
Joystick.hat(-1);
|
||||
|
||||
// Set joystick buttons to pressed
|
||||
if (fn_mode == 2){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(12, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(13, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(14, 1);
|
||||
}
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(8, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(9, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(10, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(11, 1);
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(2, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(4, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
Joystick.button(5, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(6, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(7, 1);
|
||||
}
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
Joystick.button(1, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
Joystick.button(3, 1);
|
||||
}
|
||||
if (toggle_button_arm){
|
||||
Joystick.button(15, 1);
|
||||
}
|
||||
|
||||
// Check rev mode (reverse stick for Y1 axix)
|
||||
if (joystick_y1 == AXIS_MIN){
|
||||
joystick_y1_hid_rev_mode = 1;
|
||||
}
|
||||
else if ((joystick_y1 > AXIS_MIN) && (fn_mode <= 1) && (joystick_y1_hid_rev_mode == 1)){
|
||||
joystick_y1_hid_rev_mode = 0;
|
||||
}
|
||||
else if ((joystick_y1 > AXIS_MIN) && (fn_mode > 1) && (joystick_y1_hid_rev_mode == 1)){
|
||||
joystick_y1_hid_rev_mode = 2;
|
||||
}
|
||||
|
||||
// Set axis values
|
||||
if (joystick_y1_hid_rev_mode == 0){
|
||||
Joystick.Y(joystick_y1_hid);
|
||||
}
|
||||
else if (joystick_y1_hid_rev_mode == 2){
|
||||
Joystick.Y(joystick_y1_hid_rev);
|
||||
}
|
||||
|
||||
Joystick.X(joystick_x1_hid);
|
||||
|
||||
if (fn_mode == 2){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(HID_AXIS_CENTER);
|
||||
Joystick.sliderLeft(joystick_x2_hid);
|
||||
Joystick.sliderRight(joystick_y2_hid);
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(joystick_y2_hid);
|
||||
Joystick.sliderLeft(joystick_x2_hid);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
Joystick.Z(joystick_x2_hid);
|
||||
Joystick.Zrotate(joystick_y2_hid);
|
||||
Joystick.sliderLeft(HID_AXIS_CENTER);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
|
||||
// Send USB Joystick data
|
||||
Joystick.send_now();
|
||||
send_usb_timestamp = current_timestamp + 5;
|
||||
}
|
||||
|
||||
/* Send data every CRSF_TIME_BETWEEN_FRAMES_US */
|
||||
if (current_timestamp_micros >= send_elrs_timestamp_micros)
|
||||
{
|
||||
// Set ELRS analog channels
|
||||
elrs_channels[0] = map(joystick_x1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[1] = map(joystick_y1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[2] = map(joystick_x2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[3] = map(joystick_y2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
|
||||
// Set ELRS digital channels
|
||||
for (int i = 4; i < CRSF_MAX_CHANNEL; i++){
|
||||
elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN;
|
||||
}
|
||||
if (toggle_button_arm){
|
||||
elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (toggle_button_mode){
|
||||
elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
|
||||
// Send ELRS data
|
||||
if (elrs_init_done == true){
|
||||
@ -782,16 +769,19 @@ void loop()
|
||||
ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE);
|
||||
}
|
||||
else{
|
||||
// Setting up initial communication link between ERLS TX and Teensy (give ERLS TX time to auto detect Teensy)
|
||||
if (elrs_init_counter < 500){
|
||||
crsf_prepare_data_packet(crsf_cmd_packet, elrs_channels);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
crsf_prepare_data_packet(crsf_packet, elrs_channels);
|
||||
ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX packet rate to 500Hz
|
||||
else if (elrs_init_counter < 505){
|
||||
crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX power to 100mW
|
||||
else if (elrs_init_counter < 510){
|
||||
crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
@ -801,33 +791,32 @@ void loop()
|
||||
elrs_init_done = true;
|
||||
}
|
||||
}
|
||||
elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
|
||||
send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
|
||||
}
|
||||
|
||||
/* Update indicator 200ms */
|
||||
if (current_timestamp >= indicator_timestamp)
|
||||
|
||||
/* Update leds 200ms */
|
||||
if (current_timestamp >= led_timestamp)
|
||||
{
|
||||
/* Update status led */
|
||||
if (status_led_mode == 2 && status_led_on == false)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
// Set led mode
|
||||
button_led_1_mode = 0;
|
||||
button_led_2_mode = 0;
|
||||
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
button_led_1_mode = 1;
|
||||
button_led_2_mode = 1;
|
||||
}
|
||||
else if (status_led_mode == 2 && status_led_on == true)
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
else if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
button_led_1_mode = 2;
|
||||
button_led_2_mode = 0;
|
||||
}
|
||||
else if (status_led_mode == 1)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
else if (joystick_calibration_mode == CALIBRATION_MINMAX){
|
||||
button_led_1_mode = 0;
|
||||
button_led_2_mode = 2;
|
||||
}
|
||||
else if (joystick_y1_hid_rev_mode == 2){
|
||||
button_led_1_mode = 2;
|
||||
button_led_2_mode = 2;
|
||||
}
|
||||
|
||||
/* Updated button led 1 */
|
||||
if (button_led_1_mode == 2 && button_led_1_on == false)
|
||||
@ -873,6 +862,34 @@ void loop()
|
||||
button_led_2_on = false;
|
||||
}
|
||||
|
||||
led_timestamp = current_timestamp + 200;
|
||||
}
|
||||
|
||||
/* Update indicator 200ms */
|
||||
if (current_timestamp >= indicator_timestamp)
|
||||
{
|
||||
/* Update status led */
|
||||
if (status_led_mode == 2 && status_led_on == false)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
}
|
||||
else if (status_led_mode == 2 && status_led_on == true)
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
}
|
||||
else if (status_led_mode == 1)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
}
|
||||
|
||||
indicator_timestamp = current_timestamp + 200;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user