diff --git a/firmware/Makefile b/firmware/Makefile deleted file mode 100644 index 5bb8443..0000000 --- a/firmware/Makefile +++ /dev/null @@ -1,25 +0,0 @@ -# Uncomment lines below if you have problems with $PATH -#SHELL := /bin/bash -#PATH := /usr/local/bin:$(PATH) - -all: - platformio run - -upload: - platformio run --target upload - -clean: - platformio run --target clean - -program: - platformio run --target program - -uploadfs: - platformio run --target uploadfs - -update: - platformio update - -bear: - platformio run --target clean - bear make all diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 7010a9f..123f8f0 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -26,99 +26,150 @@ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR * A PARTICULAR PURPOSE. See the GNU General Public License for more details. * - * Joystick based on standard teensy "Keypad" library for button scanning, standard teensy - * "usb_joystick" library for HID joystick usb data communication. + * Joystick based on standard teensy "usb_joystick" library for HID joystick usb data communication. * - * Layer 0 + * USB Joystick Layer 0 * - * | B7 | | B9 | - * --------------------------------------------- - * | B8 | | | B1 | B2 | | | B10 | - * | X1,Y1 | | B3 | B4 | | X2,Y2 | - * | Fn1 | | B5 |Y1 offset| B6 | | Fn2 | - * --------------------------------------------- + * | B1 | | B3 | + * | B2 | | B4 | + * -------------------------------------------- + * | | Fn1 | | B5 | | + * | | + * | X1,Y1 X2,Y2 | + * | | B6 | | B7 | | + * -------------------------------------------- * - * Layer 1 (Fn1) + * USB Joystick Layer 1 (Fn1) * - * | B17| | B19| - * --------------------------------------------- - * | B18 | | | B11| B12| | | B20 | - * | X1,Y1 | | B13| B14| | X3,Y2 | - * | Fn1 | | B15|Y1 offset| B16| | Fn2 | - * --------------------------------------------- + * | B1 | | B3 | + * | B8 | | B9 | + * -------------------------------------------- + * | | Fn1 | | Fn2 | | + * | | + * | X1,Y1 Fn1 toggle Fn1 toggle X3,Y2 | + * | | B10 | | B11 | | + * -------------------------------------------- + * + * USB Joystick Layer 2 (Fn2) * - * Layer 2 (Fn2) - * - * | B27| | B29| - * --------------------------------------------- - * | B28 | | | B21| B22| | | B30 | - * | X1,Y1 | | B23| B24| | X3,Y3 | - * | Fn1 | | B25|Y1 offset| B26| | Fn2 | - * --------------------------------------------- + * | B1 | | B3 | + * | B12 | | B13 | + * -------------------------------------------- + * | | Fn1 | | Fn2 | | + * | | + * | X1,Y1 X3,Y3 | + * | | B14 | | B15 | | + * -------------------------------------------- + * + * ELRS Layer * + * | CH7 | | CH9 | + * | HC8 | | CH10 | + * -------------------------------------------- + * | | Fn | | CH11 | | + * | | + * | X,Y Fn toggle Fn toggle X,Y | + * | CH1,CH2 | CH5 | | CH6 | CH3,CH4 | + * -------------------------------------------- + * * Features: * - * * 5 physical axis. - * * 6 joystick axis using layers. - * * 12 physical buttons. - * * 32 joystick buttons using three layers (Fn1 and Fn2). + * * USB HID Joystick. + * * ERLS protocol (CRSF) support. + * * 4 physical axis. + * * 6 joystick axis using layers (USB Joystick). + * * 8 physical buttons. + * * 15 joystick buttons using three layers (Fn1 and Fn2). */ #include -#include #include -#define USB_LED_NUM_LOCK 0 -#define USB_LED_CAPS_LOCK 1 -#define USB_LED_SCROLL_LOCK 2 +// CRSF +#define CRSF_MAX_CHANNEL 16 +#define CRSF_TYPE_CHANNELS 0x16 +#define CRSF_DIGITAL_CHANNEL_MIN 172 +#define CRSF_DIGITAL_CHANNEL_CENTER 992 +#define CRSF_DIGITAL_CHANNEL_MAX 1811 +#define CRSF_TIME_BETWEEN_FRAMES_US 1666 // 1.6 ms 500Hz +#define CRSF_PACKET_SIZE 26 +#define CRSF_CMD_PACKET_SIZE 8 -#define KEY_OFFSET 0xAA00 // Offset to apply for not interfere with already defined keyboard keys +// ELRS command +#define ELRS_ADDRESS 0xEE +#define ELRS_BIND_COMMAND 0xFF +#define ELRS_WIFI_COMMAND 0xFE +#define ELRS_PKT_RATE_COMMAND 0x01 +#define ELRS_TLM_RATIO_COMMAND 0x02 +#define ELRS_SWITCH_MODE_COMMAND 0x03 +#define ELRS_MODEL_MATCH_COMMAND 0x04 +#define ELRS_POWER_COMMAND 0x06 +#define ELRS_BLE_JOYSTIC_COMMAND 17 +#define ELRS_TYPE_SETTINGS_WRITE 0x2D +#define ELRS_ADDR_RADIO 0xEA +#define ELRS_SERIAL_PORT Serial1 +#define ELRS_SERIAL_BAUDRATE 400000 +#define ELRS_PKT_RATE 3 // 0 = 50Hz, 1 = 100Hz, 2 = 200Hz, 3 = 500Hz, 4 = 1000Hz +#define ELRS_POWER 2 // 0 = 10mW, 1 = 25mW, 2 = 100mW, 3 = 250mW, 4 = 500mW, 5 = 1000mW -#define NO_JOY 0 +// crc implementation from CRSF protocol document rev7 +static uint8_t crsf_crc8tab[256] = { + 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, + 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, + 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, + 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, + 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, + 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, + 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, + 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, + 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, + 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, + 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, + 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, + 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, + 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, + 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, + 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9}; -#define JOY_A1 1 + KEY_OFFSET -#define JOY_A2 2 + KEY_OFFSET -#define JOY_A3 3 + KEY_OFFSET -#define JOY_A4 4 + KEY_OFFSET -#define JOY_A5 5 + KEY_OFFSET -#define JOY_A6 6 + KEY_OFFSET -#define JOY_A7 7 + KEY_OFFSET -#define JOY_A8 8 + KEY_OFFSET -#define JOY_A9 9 + KEY_OFFSET -#define JOY_A10 10 + KEY_OFFSET -#define JOY_A11 11 + KEY_OFFSET -#define JOY_A12 12 + KEY_OFFSET -#define JOY_A13 13 + KEY_OFFSET -#define JOY_A14 14 + KEY_OFFSET -#define JOY_A15 15 + KEY_OFFSET -#define JOY_A16 16 + KEY_OFFSET -#define JOY_A17 17 + KEY_OFFSET -#define JOY_A18 18 + KEY_OFFSET -#define JOY_A19 19 + KEY_OFFSET -#define JOY_A20 20 + KEY_OFFSET -#define JOY_A21 21 + KEY_OFFSET -#define JOY_A22 22 + KEY_OFFSET -#define JOY_A23 23 + KEY_OFFSET -#define JOY_A24 24 + KEY_OFFSET -#define JOY_A25 25 + KEY_OFFSET -#define JOY_A26 26 + KEY_OFFSET -#define JOY_A27 27 + KEY_OFFSET -#define JOY_A28 28 + KEY_OFFSET -#define JOY_A29 29 + KEY_OFFSET -#define JOY_A30 30 + KEY_OFFSET -#define JOY_A31 31 + KEY_OFFSET -#define JOY_A32 32 + KEY_OFFSET +uint8_t crsf_packet[CRSF_PACKET_SIZE]; +uint8_t crsf_cmd_packet[CRSF_CMD_PACKET_SIZE]; +int16_t elrs_channels[CRSF_MAX_CHANNEL]; +bool elrs_init_done = false; +int elrs_init_counter = 0; -#define JOY_AHU1 49 + KEY_OFFSET -#define JOY_AHR1 50 + KEY_OFFSET -#define JOY_AHD1 51 + KEY_OFFSET -#define JOY_AHL1 52 + KEY_OFFSET +const int STATUS_LED = 13; +bool status_led_on = false; +int status_led_mode = 2; -#define KEY_FN1 61 + KEY_OFFSET // Function layer 1 button -#define KEY_FN2 62 + KEY_OFFSET // Function layer 2 button +const int BUTTON_LED_1 = 22; +bool button_led_1_on = false; +int button_led_1_mode = 0; -#define TAP_TIMEOUT 150 // Key tap timeout (ms) -#define NBR_OF_BUTTONS 12 // Number of buttons used (12 in this case) +const int BUTTON_LED_2 = 23; +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 indicator_timestamp = 0; + +unsigned long current_timestamp_micros = 0; +unsigned long elrs_timestamp_micros = 0; + +const int BUTTON_FLU = 4; // Front left upper button +const int BUTTON_FLD = 5; // Front left lower button +const int BUTTON_FRU = 6; // Front right upper button +const int BUTTON_FRD = 7; // Front right lower button +const int BUTTON_TLU = 9; // top upper left button +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; +int button_11_previous_value = HIGH; #define HID_AXIS_MAX 1023 #define HID_AXIS_MIN 0 @@ -129,90 +180,6 @@ #define DEADZONE_X 50 #define DEADZONE_Y 50 -struct Button -{ - int keypad_kchar = 0; - uint16_t keycode = NO_KEY; - uint16_t tap_keycode = NO_KEY; - uint16_t fn1_keycode = NO_KEY; - uint16_t fn2_keycode = NO_KEY; - bool hold_direct = true; - int kstate = IDLE; - uint16_t last_keycode = NO_KEY; - bool run_keycode = false; - int tap_state = 0; - bool tap_timeout_enable = false; - bool tap_release_enable = false; - unsigned long tap_timeout_timestamp = 0; - unsigned long tap_release_timestamp = 0; - bool tap_inhibit = false; -}; - -const byte KP_ROWS = 3; -const byte KP_COLS = 4; - -byte kp_rowPins[KP_ROWS] = {6, 7, 8}; -byte kp_colPins[KP_COLS] = {9, 10, 11, 12}; - -char kp_keys[KP_ROWS][KP_COLS] = { - {1, 2, 3, 4}, - {5, 6, 7, 8}, - {9, 10, 11, 12}}; - -Keypad kp_keypad = Keypad(makeKeymap(kp_keys), kp_rowPins, kp_colPins, KP_ROWS, KP_COLS); - - -/* Valid "Fn0 (hold) key" when using tap mode are: KEY_LEFT_SHIFT, KEY_RIGHT_SHIFT, KEY_LEFT_CTRL, KEY_RIGHT_CTRL, KEY_RIGHT_ALT, KEY_LEFT_GUI, KEY_RIGHT_GUI, KEY_FN1, KEY_FN2 - - * "Button ID" corresponding with the physical design of the actual keyboard. DO NOT CHANGE BTN ID! - - | 1 | | 4 | - --------------------------------------------- - | 5 | | | 2 | 3 | | | 8 | - | X1,Y1 | | 6 | 7 | | X3,Y3 | - | 9 | | 10 |Y1 offset| 11 | | 12 | - --------------------------------------------- - - * "Fn0 hold key" is normal key in non tap mode. In tap mode this key is the hold key. - * "Fn0 tap key" enables tap mode. - * "Fn1 key" is the layer 1 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer. - * "Fn2 key" is the layer 2 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer. - "Fn1 key" and "Fn2 key" are N/A when using tap mode and should me defined as NO_KEY. - * "Hold direct" enables sending PRESS command as soon as the hold key is pressed (regardless if you intend to press the tap key). - -/* Keymap config ----------------------------------------------------------------------------------------------------------------------------------- */ -// clang-format off -Button buttons[NBR_OF_BUTTONS] = - { -/* Btn ID Fn0 (hold) key Fn1 key Fn2 key */ - {1, JOY_A7, JOY_A17, JOY_A27, IDLE, NO_KEY, false}, - {2, JOY_A1, JOY_A11, JOY_A21, IDLE, NO_KEY, false}, - {3, JOY_A2, JOY_A12, JOY_A22, IDLE, NO_KEY, false}, - {4, JOY_A9, JOY_A19, JOY_A29, IDLE, NO_KEY, false}, - {5, JOY_A8, JOY_A18, JOY_A28, IDLE, NO_KEY, false}, - {6, JOY_A3, JOY_A13, JOY_A23, IDLE, NO_KEY, false}, - {7, JOY_A4, JOY_A14, JOY_A24, IDLE, NO_KEY, false}, - {8, JOY_A10, JOY_A20, JOY_A30, IDLE, NO_KEY, false}, - {9, KEY_FN1, NO_KEY, NO_KEY, IDLE, NO_KEY, false}, - {10, JOY_A5, JOY_A15, JOY_A25, IDLE, NO_KEY, false}, - {11, JOY_A6, JOY_A16, JOY_A26, IDLE, NO_KEY, false}, - {12, KEY_FN2, NO_KEY, NO_KEY, IDLE, NO_KEY, false}}; - -// clang-format on -/* End of keymap config ----------------------------------------------------------------------------------------------------------------------------- */ - -const int STATUS_LED = 13; -bool status_led_on = false; -int status_led_mode = 0; - -unsigned long current_timestamp = 0; -unsigned long button_timestamp = 0; -unsigned long indicator_timestamp = 0; - -bool key_pressed = false; - -int fn_mode = 0; - // USB HID data variables int xy_x1 = HID_AXIS_CENTER; int xy_y1 = HID_AXIS_CENTER; @@ -247,12 +214,6 @@ enum EEPROM_ADR MIN_Y2_ADR_LOW, CNT_Y2_ADR_HIGH, CNT_Y2_ADR_LOW, - MAX_X3_ADR_HIGH, - MAX_X3_ADR_LOW, - MIN_X3_ADR_HIGH, - MIN_X3_ADR_LOW, - CNT_X3_ADR_HIGH, - CNT_X3_ADR_LOW, EEPROM_ADR_NBR_OF_BYTES }; @@ -277,18 +238,16 @@ int joystick_y2_raw = 0; int joystick_y2_max = 4096; int joystick_y2_min = 0; int joystick_y2_center = joystick_y2_max/2; -int joystick_x3 = 0; -int joystick_x3_raw = 0; -int joystick_x3_max = 4096; -int joystick_x3_min = 0; -int joystick_x3_center = joystick_x3_max/2; float exp_constant = 0.2; #define CALIBRATION_OFF 0 -#define CALIBRATION_CENTER 1 -#define CALIBRATION_MINMAX 2 -int joystick_calibration_mode = 0; +#define CALIBRATION_INIT 1 +#define CALIBRATION_CENTER 2 +#define CALIBRATION_MINMAX 3 +int joystick_calibration_mode = CALIBRATION_OFF; + +// ----------------------------------------------- void save_to_eeprom(){ EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_max); @@ -318,14 +277,6 @@ void save_to_eeprom(){ EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_min >> 8); EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_center); EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_center >> 8); - - EEPROM.write(MAX_X3_ADR_LOW, joystick_x3_max); - EEPROM.write(MAX_X3_ADR_HIGH, joystick_x3_max >> 8); - EEPROM.write(MIN_X3_ADR_LOW, joystick_x3_min); - EEPROM.write(MIN_X3_ADR_HIGH, joystick_x3_min >> 8); - EEPROM.write(CNT_X3_ADR_LOW, joystick_x3_center); - EEPROM.write(CNT_X3_ADR_HIGH, joystick_x3_center >> 8); - } void load_from_eeprom(){ @@ -356,17 +307,197 @@ void load_from_eeprom(){ joystick_y2_min |= EEPROM.read(MIN_Y2_ADR_LOW); joystick_y2_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8); joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW); - - joystick_x3_max = (EEPROM.read(MAX_X3_ADR_HIGH) << 8); - joystick_x3_max |= EEPROM.read(MAX_X3_ADR_LOW); - joystick_x3_min = (EEPROM.read(MIN_X3_ADR_HIGH) << 8); - joystick_x3_min |= EEPROM.read(MIN_X3_ADR_LOW); - joystick_x3_center = (EEPROM.read(CNT_X3_ADR_HIGH) << 8); - joystick_x3_center |= EEPROM.read(CNT_X2_ADR_LOW); - } -void update_analog(){ +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){ joystick_x1_raw = analogRead(0); @@ -382,17 +513,11 @@ void update_analog(){ } else if(joystick_counter == 3){ joystick_y2_raw = analogRead(3); - joystick_counter++; - } - else if(joystick_counter == 4){ - joystick_x3_raw = analogRead(4); joystick_counter = 0; if (joystick_calibration_mode == CALIBRATION_OFF){ - // ---------------------------------------------------------- - // Map X1 joystick values to proper HID values - // ---------------------------------------------------------- + // Calculate X1 joystick values if(joystick_x1_raw > (joystick_x1_center + DEADZONE_X)){ joystick_x1 = constrain(map(joystick_x1_raw, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); } @@ -402,9 +527,8 @@ void update_analog(){ else{ joystick_x1 = AXIS_CENTER; } - // ---------------------------------------------------------- - // Map Y1 joystick values to proper HID values - // ---------------------------------------------------------- + + // 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); } @@ -415,9 +539,7 @@ void update_analog(){ joystick_y1 = AXIS_CENTER; } - // ---------------------------------------------------------- - // Map X2 joystick values to proper HID values - // ---------------------------------------------------------- + // Calculate X2 joystick values if(joystick_x2_raw > (joystick_x2_center + DEADZONE_X)){ joystick_x2 = constrain(map(joystick_x2_raw, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); } @@ -427,9 +549,8 @@ void update_analog(){ else{ joystick_x2 = AXIS_CENTER; } - // ---------------------------------------------------------- - // Map Y1 joystick values to proper HID values - // ---------------------------------------------------------- + + // Calculate Y2 joystick values if(joystick_y2_raw > (joystick_y2_center + DEADZONE_Y)){ joystick_y2 = constrain(map(joystick_y2_raw, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); } @@ -440,25 +561,7 @@ void update_analog(){ joystick_y2 = AXIS_CENTER; } - // ---------------------------------------------------------- - // Map X3 joystick values to proper HID values - // ---------------------------------------------------------- - if(joystick_x3_raw > (joystick_x3_center + DEADZONE_X)){ - joystick_x3 = constrain(map(joystick_x3_raw, (joystick_x3_center + DEADZONE_X), joystick_x3_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); - } - else if(joystick_x3_raw < (joystick_x3_center - DEADZONE_X)){ - joystick_x3 = constrain(map(joystick_x3_raw, joystick_x3_min, (joystick_x3_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); - } - else{ - joystick_x3 = AXIS_CENTER; - } - - // ---------------------------------------------------------- - // Calculate new axis values after applying exp curve - // ---------------------------------------------------------- - // Normal mode - exp_constant = 0.2; - + // Calculate new X1 values after applying exp curve 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; @@ -468,6 +571,8 @@ void update_analog(){ else{ joystick_x1 = 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; @@ -477,7 +582,8 @@ void update_analog(){ else{ joystick_y1 = HID_AXIS_CENTER; } - + + // 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; @@ -487,6 +593,8 @@ void update_analog(){ else{ joystick_x2 = 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; @@ -496,28 +604,16 @@ void update_analog(){ else{ joystick_y2 = HID_AXIS_CENTER; } - - if (joystick_x3 != AXIS_CENTER){ - float joystick_x_float = joystick_x3 / 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_x3 = int(joystick_x_exp * float(HID_AXIS_MAX)); - joystick_x3 = constrain(joystick_x3, HID_AXIS_MIN, HID_AXIS_MAX); - } - else{ - joystick_x3 = HID_AXIS_CENTER; - } } else{ - // ---------------------------------------------------------- // Calibration mode. - // ---------------------------------------------------------- - joystick_x1 = HID_AXIS_CENTER; joystick_x1 = HID_AXIS_CENTER; + joystick_y1 = HID_AXIS_CENTER; joystick_x2 = HID_AXIS_CENTER; joystick_y2 = HID_AXIS_CENTER; - joystick_x3 = HID_AXIS_CENTER; + // Calibrate joystick center values if (joystick_calibration_mode == CALIBRATION_CENTER){ joystick_x1_center = joystick_x1_raw; joystick_y1_center = joystick_y1_raw; @@ -532,11 +628,8 @@ void update_analog(){ joystick_x2_min = joystick_x2_center; joystick_y2_max = joystick_y2_center; joystick_y2_min = joystick_y2_center; - - joystick_x3_center = joystick_x3_raw; - joystick_x3_max = joystick_x3_center; - joystick_x3_min = joystick_x3_center; } + // Calibrate joystick min/max values else if (joystick_calibration_mode == CALIBRATION_MINMAX){ if(joystick_x1_raw > joystick_x1_max){ joystick_x1_max = joystick_x1_raw; @@ -563,220 +656,60 @@ void update_analog(){ if(joystick_y2_raw < joystick_y2_min){ joystick_y2_min = joystick_y2_raw; } - - if(joystick_x3_raw > joystick_x3_max){ - joystick_x3_max = joystick_x3_raw; - } - if(joystick_x3_raw < joystick_x3_min){ - joystick_x3_min = joystick_x3_raw; - } } } } } - -/** - Perform key action. - - @param keycode code to apply action. - @param kstate PRESSED or RELEASED. - @return Action applied. -*/ -bool set_key(uint16_t keycode, uint8_t kstate) -{ - /* Abort if keycode is invalid */ - if (keycode == NO_KEY || keycode == KEY_FN1 || keycode == KEY_FN2) - { - return false; +// CRSF CRC calculation via lookup table +uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) { + uint8_t crc = 0; + for (uint8_t i = 0; i < len; i++){ + crc = crsf_crc8tab[crc ^ *ptr++]; } - - /* Joystick buttons */ - else if (keycode >= JOY_A1 && keycode <= JOY_A32) { - if (kstate == RELEASED) { - Joystick.button(keycode-KEY_OFFSET, false); - } else if (kstate == PRESSED) { - Joystick.button(keycode-KEY_OFFSET, true); - } - } - - /* Joystick hat */ - else if (keycode >= JOY_AHU1 && keycode <= JOY_AHL1) { - if (kstate == RELEASED) { - Joystick.hat(-1); - } else if (kstate == PRESSED) { - Joystick.hat(((keycode-KEY_OFFSET)-(JOY_AHU1-KEY_OFFSET)) * 90); - } - } - - return true; + return crc; } -/** - Scan key matrix and perform processing for each key. +// Prepare CRSF data packet +void crsf_prepare_data_packet(uint8_t packet[], int16_t channels[]) { + packet[0] = ELRS_ADDRESS; // Header + packet[1] = 24; // length + packet[2] = CRSF_TYPE_CHANNELS; + packet[3] = (uint8_t)(channels[0] & 0x07FF); + packet[4] = (uint8_t)((channels[0] & 0x07FF) >> 8 | (channels[1] & 0x07FF) << 3); + packet[5] = (uint8_t)((channels[1] & 0x07FF) >> 5 | (channels[2] & 0x07FF) << 6); + packet[6] = (uint8_t)((channels[2] & 0x07FF) >> 2); + packet[7] = (uint8_t)((channels[2] & 0x07FF) >> 10 | (channels[3] & 0x07FF) << 1); + packet[8] = (uint8_t)((channels[3] & 0x07FF) >> 7 | (channels[4] & 0x07FF) << 4); + packet[9] = (uint8_t)((channels[4] & 0x07FF) >> 4 | (channels[5] & 0x07FF) << 7); + packet[10] = (uint8_t)((channels[5] & 0x07FF) >> 1); + packet[11] = (uint8_t)((channels[5] & 0x07FF) >> 9 | (channels[6] & 0x07FF) << 2); + packet[12] = (uint8_t)((channels[6] & 0x07FF) >> 6 | (channels[7] & 0x07FF) << 5); + packet[13] = (uint8_t)((channels[7] & 0x07FF) >> 3); + packet[14] = (uint8_t)((channels[8] & 0x07FF)); + packet[15] = (uint8_t)((channels[8] & 0x07FF) >> 8 | (channels[9] & 0x07FF) << 3); + packet[16] = (uint8_t)((channels[9] & 0x07FF) >> 5 | (channels[10] & 0x07FF) << 6); + packet[17] = (uint8_t)((channels[10] & 0x07FF) >> 2); + packet[18] = (uint8_t)((channels[10] & 0x07FF) >> 10 | (channels[11] & 0x07FF) << 1); + packet[19] = (uint8_t)((channels[11] & 0x07FF) >> 7 | (channels[12] & 0x07FF) << 4); + packet[20] = (uint8_t)((channels[12] & 0x07FF) >> 4 | (channels[13] & 0x07FF) << 7); // Channel not used by ELRS + packet[21] = (uint8_t)((channels[13] & 0x07FF) >> 1); // Channel not used by ELRS + packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); // Channel not used by ELRS + packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); // Channel not used by ELRS + packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); // Channel not used by ELRS + packet[25] = crsf_crc8(&packet[2], packet[1] - 1); +} - @return void. -*/ -void scan_buttons() -{ - /* Scan keypad */ - if (kp_keypad.getKeys()) - { - - /* Enter bootloader if all reboot-buttons is pressed together */ - int reboot = 0; - for (int i = 0; i < LIST_MAX; i++) - { - if ((kp_keypad.key[i].kchar == 1) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - reboot += 1; - } - if ((kp_keypad.key[i].kchar == 4) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - reboot += 1; - } - if ((kp_keypad.key[i].kchar == 10) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - reboot += 1; - } - if ((kp_keypad.key[i].kchar == 11) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - reboot += 1; - } - } - if (reboot == 4) - { - _reboot_Teensyduino_(); - } - - /* Enter calibration mode if all calibration-buttons is pressed together */ - int calibrate = 0; - for (int i = 0; i < LIST_MAX; i++) - { - if ((kp_keypad.key[i].kchar == 2) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - calibrate += 1; - } - if ((kp_keypad.key[i].kchar == 3) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - calibrate += 1; - } - if ((kp_keypad.key[i].kchar == 10) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - calibrate += 1; - } - if ((kp_keypad.key[i].kchar == 11) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)) - { - calibrate += 1; - } - } - if (calibrate == 4) - { - joystick_calibration_mode = CALIBRATION_CENTER; - } - - /* Check for Fn mode */ - fn_mode = 0; - for (int i = 0; i < LIST_MAX; i++) - { - if (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD) - { - for (int j = 0; j < NBR_OF_BUTTONS; j++) - { - if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar) - { - /* Check if FN1 key are defined to this button (Layer 0 and first position in combo array)*/ - if (buttons[j].keycode == KEY_FN1) - { - fn_mode++; - } - break; - } - } - } - } - - /* Process key press/release */ - for (int i = 0; i < LIST_MAX; i++) - { - if (kp_keypad.key[i].kstate == PRESSED) - { - for (int j = 0; j < NBR_OF_BUTTONS; j++) - { - if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true) - { - buttons[j].run_keycode = true; - buttons[j].kstate = PRESSED; - break; - } - } - } - else if (kp_keypad.key[i].kstate == RELEASED) - { - for (int j = 0; j < NBR_OF_BUTTONS; j++) - { - if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true) - { - buttons[j].run_keycode = true; - buttons[j].kstate = RELEASED; - break; - } - } - } - } - - - /* Check if any "non tap keys" has been pressed */ - for (int i = 0; i < NBR_OF_BUTTONS; i++) - { - if (buttons[i].run_keycode == true && buttons[i].tap_keycode == NO_KEY && buttons[i].kstate == PRESSED) - { - for (int j = 0; j < NBR_OF_BUTTONS; j++) - { - if (buttons[j].tap_keycode != NO_KEY) - { - buttons[j].tap_inhibit = true; - } - } - } - } - - /* Execute key commands */ - for (int i = 0; i < NBR_OF_BUTTONS; i++) - { - /* Check if key should be processed */ - if (buttons[i].run_keycode == true) - { - /* Check if key pressed or released */ - if (buttons[i].kstate == PRESSED) - { - if (fn_mode == 0) - { - set_key(buttons[i].keycode, PRESSED); - buttons[i].last_keycode = buttons[i].keycode; - } - else if (fn_mode == 1) - { - set_key(buttons[i].fn1_keycode, PRESSED); - buttons[i].last_keycode = buttons[i].fn1_keycode; - } - else if (fn_mode == 2) - { - set_key(buttons[i].fn2_keycode, PRESSED); - buttons[i].last_keycode = buttons[i].fn2_keycode; - } - } - else if (buttons[i].kstate == RELEASED) - { - /* Sending release command for last keycode related to this button */ - set_key(buttons[i].last_keycode, RELEASED); - } - /* Reset run_keycode flag */ - buttons[i].run_keycode = false; - } - } - - } +// Prepare CRSF setup packet +void crsf_prepare_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) { + packet_cmd[0] = ELRS_ADDRESS; // Header + packet_cmd[1] = 6; // length + packet_cmd[2] = ELRS_TYPE_SETTINGS_WRITE; + packet_cmd[3] = ELRS_ADDRESS; + packet_cmd[4] = ELRS_ADDR_RADIO; + packet_cmd[5] = command; + packet_cmd[6] = value; + packet_cmd[7] = crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); } void setup() @@ -784,59 +717,97 @@ void setup() /* Init HW */ pinMode(STATUS_LED, OUTPUT); digitalWrite(STATUS_LED, LOW); + + pinMode(BUTTON_LED_1, OUTPUT); + digitalWrite(BUTTON_LED_1, LOW); + + pinMode(BUTTON_LED_2, OUTPUT); + digitalWrite(BUTTON_LED_2, LOW); + + pinMode(BUTTON_FLD, INPUT_PULLUP); + pinMode(BUTTON_FLU, INPUT_PULLUP); + pinMode(BUTTON_FRD, INPUT_PULLUP); + pinMode(BUTTON_FRU, INPUT_PULLUP); + pinMode(BUTTON_TLD, INPUT_PULLUP); + pinMode(BUTTON_TRD, INPUT_PULLUP); + pinMode(BUTTON_TLU, INPUT_PULLUP); + pinMode(BUTTON_TRU, INPUT_PULLUP); // Set ADC resolution to 12bit analogReadResolution(12); analogReadAveraging(32); delay(500); + // Init EEPROM + load_from_eeprom(); + // Init Joystick Joystick.useManualSend(true); + // Check if calibration mode is enabled + if (digitalRead(BUTTON_TLD) == LOW){ + joystick_calibration_mode = CALIBRATION_INIT; + } + + // Check if bootloader mode is enabled + if (digitalRead(BUTTON_TRD) == LOW){ + _reboot_Teensyduino_(); + } + + // Init ELRS + ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE); } void loop() { /* Update current time (ms) */ current_timestamp = millis(); + /* Update current time (us) */ + current_timestamp_micros = micros(); - /* Scan buttons 1ms */ - if (current_timestamp >= button_timestamp) + /* Read io value and process as fast as possible */ + read_io_data(); + + /* Send data every 1500us */ + if (current_timestamp_micros >= elrs_timestamp_micros) { - button_timestamp = current_timestamp + 100; - update_analog(); - //scan_buttons(); - - // ---------------------------------------------------------- - // Update joystick axis data - // ---------------------------------------------------------- - 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(joystick_x3); - Joystick.sliderRight(joystick_x3); - } + process_io_data(); + + // Send USB Joystick data Joystick.send_now(); + + // Send ELRS data + if (elrs_init_done == true){ + crsf_prepare_data_packet(crsf_packet, elrs_channels); + ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE); + } + else{ + 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); + elrs_init_counter++; + } + 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++; + } + 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); + elrs_init_counter++; + } + else{ + elrs_init_done = true; + } + } + elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; } /* Update indicator 200ms */ if (current_timestamp >= indicator_timestamp) { + /* Update status led */ if (status_led_mode == 2 && status_led_on == false) { digitalWrite(STATUS_LED, HIGH); @@ -858,6 +829,50 @@ void loop() status_led_on = false; } + /* Updated button led 1 */ + if (button_led_1_mode == 2 && button_led_1_on == false) + { + digitalWrite(BUTTON_LED_1, HIGH); + button_led_1_on = true; + } + else if (button_led_1_mode == 2 && button_led_1_on == true) + { + digitalWrite(BUTTON_LED_1, LOW); + button_led_1_on = false; + } + else if (button_led_1_mode == 1) + { + digitalWrite(BUTTON_LED_1, HIGH); + button_led_1_on = true; + } + else + { + digitalWrite(BUTTON_LED_1, LOW); + button_led_1_on = false; + } + + /* Updated button led 2 */ + if (button_led_2_mode == 2 && button_led_2_on == false) + { + digitalWrite(BUTTON_LED_2, HIGH); + button_led_2_on = true; + } + else if (button_led_2_mode == 2 && button_led_2_on == true) + { + digitalWrite(BUTTON_LED_2, LOW); + button_led_2_on = false; + } + else if (button_led_2_mode == 1) + { + digitalWrite(BUTTON_LED_2, HIGH); + button_led_2_on = true; + } + else + { + digitalWrite(BUTTON_LED_2, LOW); + button_led_2_on = false; + } + indicator_timestamp = current_timestamp + 200; } }