diff --git a/firmware/src/ElrsTx.cpp b/firmware/src/ElrsTx.cpp index 4694ed1..cda6ba8 100644 --- a/firmware/src/ElrsTx.cpp +++ b/firmware/src/ElrsTx.cpp @@ -34,19 +34,18 @@ * * @param serial The serial port to use */ -ElrsTx::ElrsTx(HardwareSerial& serial): _serial(serial) { } +ElrsTx::ElrsTx(HardwareSerial &serial) : _serial(serial) {} /** - * @brief Initialize the ELRS transmitter + * @brief Initialize the ELRS transmitter */ -void ElrsTx::begin(int packet_rate, int tx_power) -{ - const int ELRS_SERIAL_BAUDRATE = 400000; - - _serial.begin(ELRS_SERIAL_BAUDRATE); +void ElrsTx::begin(int packet_rate, int tx_power) { + const int ELRS_SERIAL_BAUDRATE = 400000; - _packet_rate = packet_rate; - _tx_power = tx_power; + _serial.begin(ELRS_SERIAL_BAUDRATE); + + _packet_rate = packet_rate; + _tx_power = tx_power; } /** @@ -56,14 +55,12 @@ void ElrsTx::begin(int packet_rate, int tx_power) * @param len The length of the packet * @return The CRC8 of the packet */ -uint8_t ElrsTx::calculate_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++]; - } - return crc; +uint8_t ElrsTx::calculate_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++]; + } + return crc; } /** @@ -75,40 +72,39 @@ uint8_t ElrsTx::calculate_crsf_crc8(const uint8_t *ptr, uint8_t len) * 2 - type * 3-24 - data (16 11bit channels in 8bit format) * 25 - crc - * - * @param packet Resulting packet - * @param channels Channel data + * + * @param packet Resulting packet + * @param channels Channel data */ -void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) -{ - const int CRSF_TYPE_CHANNELS = 0x16; - - packet[0] = ELRS_ADDRESS; - packet[1] = 24; - 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); - packet[21] = (uint8_t)((channels[13] & 0x07FF) >> 1); - packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); - packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); - packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); - packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1); +void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) { + const int CRSF_TYPE_CHANNELS = 0x16; + + packet[0] = ELRS_ADDRESS; + packet[1] = 24; + 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); + packet[21] = (uint8_t)((channels[13] & 0x07FF) >> 1); + packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); + packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); + packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); + packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1); } /** @@ -124,31 +120,30 @@ void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) * 6 - value * 7 - crc * - * command list: + * command list: * ELRS_BIND_COMMAND = 0xFF; * ELRS_WIFI_COMMAND = 0xFE; * ELRS_TLM_RATIO_COMMAND = 0x02; * ELRS_SWITCH_MODE_COMMAND = 0x03; * ELRS_MODEL_MATCH_COMMAND = 0x04; * ELRS_BLE_JOYSTIC_COMMAND = 17; - * - * @param packet_cmd Resulting packet - * @param command Command to be sent + * + * @param packet_cmd Resulting packet + * @param command Command to be sent * @param value Value to be sent */ -void ElrsTx::prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) -{ - const int ELRS_ADDR_RADIO = 0xEA; - const int ELRS_TYPE_SETTINGS_WRITE = 0x2D; - - packet_cmd[0] = ELRS_ADDRESS; - packet_cmd[1] = 6; - 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] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); +void ElrsTx::prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) { + const int ELRS_ADDR_RADIO = 0xEA; + const int ELRS_TYPE_SETTINGS_WRITE = 0x2D; + + packet_cmd[0] = ELRS_ADDRESS; + packet_cmd[1] = 6; + 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] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); } /** @@ -157,50 +152,38 @@ void ElrsTx::prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint * @param value Value (0-2048) to be set * @param channel Channel (0-11) to be set */ -void ElrsTx::set_data(int16_t value, uint8_t channel) -{ - elrs_channels[channel] = value; -} +void ElrsTx::set_data(int16_t value, uint8_t channel) { elrs_channels[channel] = value; } /** * @brief Send data to ERLS TX. This function should be called within the main loop with a 1.6ms interval */ -void ElrsTx::send_data() -{ - const int ELRS_PKT_RATE_COMMAND = 0x01; - const int ELRS_POWER_COMMAND = 0x06; - - if (elrs_init_done == true) - { - prepare_crsf_data_packet(crsf_packet, elrs_channels); - _serial.write(crsf_packet, CRSF_PACKET_SIZE); +void ElrsTx::send_data() { + const int ELRS_PKT_RATE_COMMAND = 0x01; + const int ELRS_POWER_COMMAND = 0x06; + + if (elrs_init_done == true) { + prepare_crsf_data_packet(crsf_packet, elrs_channels); + _serial.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) { + prepare_crsf_data_packet(crsf_packet, elrs_channels); + _serial.write(crsf_packet, CRSF_PACKET_SIZE); + elrs_init_counter++; } - else - { - // Setting up initial communication link between ERLS TX and Teensy (give ERLS TX time to auto detect Teensy) - if (elrs_init_counter < 500) - { - prepare_crsf_data_packet(crsf_packet, elrs_channels); - _serial.write(crsf_packet, CRSF_PACKET_SIZE); - elrs_init_counter++; - } - // Send command to update TX packet rate - else if (elrs_init_counter < 505) - { - prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, _packet_rate); - _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); - elrs_init_counter++; - } - // Send command to update TX power - else if (elrs_init_counter < 510) - { - prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, _tx_power); - _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); - elrs_init_counter++; - } - else - { - elrs_init_done = true; - } + // Send command to update TX packet rate + else if (elrs_init_counter < 505) { + prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, _packet_rate); + _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + elrs_init_counter++; } -} \ No newline at end of file + // Send command to update TX power + else if (elrs_init_counter < 510) { + prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, _tx_power); + _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + elrs_init_counter++; + } else { + elrs_init_done = true; + } + } +} diff --git a/firmware/src/ElrsTx.h b/firmware/src/ElrsTx.h index 65b2a50..c7b53c8 100644 --- a/firmware/src/ElrsTx.h +++ b/firmware/src/ElrsTx.h @@ -1,4 +1,4 @@ - /* +/* * ======================================================================================================= * ------------------------------------------------------------------------------------------------------- * ---####################-----###########-------###########-----############--############-############-- @@ -32,18 +32,18 @@ #include -// CRSF +// CRSF const int CRSF_MAX_CHANNEL = 16; const int CRSF_TYPE_CHANNELS = 0x16; const int CRSF_DIGITAL_CHANNEL_MIN = 172; -const int CRSF_DIGITAL_CHANNEL_CENTER = 992; +const int CRSF_DIGITAL_CHANNEL_CENTER = 992; const int CRSF_DIGITAL_CHANNEL_MAX = 1811; -const int CRSF_TIME_BETWEEN_FRAMES_US = 1666; +const int CRSF_TIME_BETWEEN_FRAMES_US = 1666; const int CRSF_PACKET_SIZE = 26; const int CRSF_CMD_PACKET_SIZE = 8; // ERLS others -const int ELRS_ADDR_RADIO = 0xEA; +const int ELRS_ADDR_RADIO = 0xEA; const int ELRS_SERIAL_BAUDRATE = 400000; // ELRS command @@ -59,10 +59,10 @@ const int ELRS_BLE_JOYSTIC_COMMAND = 17; const int ELRS_TYPE_SETTINGS_WRITE = 0x2D; // ERLS command values -const int ELRS_PACKET_RATE_50Hz = 0; -const int ELRS_PACKET_RATE_100Hz = 1; -const int ELRS_PACKET_RATE_200Hz = 2; -const int ELRS_PACKET_RATE_500Hz = 3; +const int ELRS_PACKET_RATE_50Hz = 0; +const int ELRS_PACKET_RATE_100Hz = 1; +const int ELRS_PACKET_RATE_200Hz = 2; +const int ELRS_PACKET_RATE_500Hz = 3; const int ELRS_TX_POWER_10mW = 0; const int ELRS_TX_POWER_25mW = 1; const int ELRS_TX_POWER_100mW = 2; @@ -70,50 +70,40 @@ const int ELRS_TX_POWER_250mW = 3; const int ELRS_TX_POWER_500mW = 4; const int ELRS_TX_POWER_1000mW = 5; -class ElrsTx -{ - public: - ElrsTx(HardwareSerial& serial); - - void set_data(int16_t value, uint8_t channel); - void send_data(); - void begin(int packet_rate, int tx_power); - - private: - uint8_t crsf_packet[CRSF_PACKET_SIZE]; - uint8_t crsf_cmd_packet[CRSF_CMD_PACKET_SIZE]; - int16_t elrs_channels[CRSF_MAX_CHANNEL]; +class ElrsTx { +public: + ElrsTx(HardwareSerial &serial); - bool elrs_init_done = false; - int elrs_init_counter = 0; - - HardwareSerial& _serial; - int _packet_rate; - int _tx_power; + void set_data(int16_t value, uint8_t channel); + void send_data(); + void begin(int packet_rate, int tx_power); - // crc implementation from CRSF protocol document rev7 - const 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}; +private: + uint8_t crsf_packet[CRSF_PACKET_SIZE]; + uint8_t crsf_cmd_packet[CRSF_CMD_PACKET_SIZE]; + int16_t elrs_channels[CRSF_MAX_CHANNEL]; - uint8_t calculate_crsf_crc8(const uint8_t *ptr, uint8_t len); - void prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]); - void prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value); + bool elrs_init_done = false; + int elrs_init_counter = 0; + HardwareSerial &_serial; + int _packet_rate; + int _tx_power; + + // crc implementation from CRSF protocol document rev7 + const 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}; + + uint8_t calculate_crsf_crc8(const uint8_t *ptr, uint8_t len); + void prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]); + void prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value); }; -#endif \ No newline at end of file +#endif diff --git a/firmware/src/IndicatorLed.cpp b/firmware/src/IndicatorLed.cpp index 3001d73..34774bf 100644 --- a/firmware/src/IndicatorLed.cpp +++ b/firmware/src/IndicatorLed.cpp @@ -31,12 +31,12 @@ /** * @brief Construct a new Indicator Led:: Indicator Led object - * - * @param pin Pin number for indicator LED + * + * @param pin Pin number for indicator LED */ IndicatorLed::IndicatorLed(int pin) { - _pin = pin; + _pin = pin; } /** @@ -44,8 +44,8 @@ IndicatorLed::IndicatorLed(int pin) */ void IndicatorLed::begin() { - pinMode(_pin, OUTPUT); - digitalWrite(_pin, LOW); + pinMode(_pin, OUTPUT); + digitalWrite(_pin, LOW); } /** @@ -53,7 +53,7 @@ void IndicatorLed::begin() */ void IndicatorLed::on() { - led_mode = LED_ON; + led_mode = LED_ON; } /** @@ -61,7 +61,7 @@ void IndicatorLed::on() */ void IndicatorLed::off() { - led_mode = LED_OFF; + led_mode = LED_OFF; } /** @@ -69,7 +69,7 @@ void IndicatorLed::off() */ void IndicatorLed::blink() { - led_mode = LED_BLINK; + led_mode = LED_BLINK; } /** @@ -78,24 +78,24 @@ void IndicatorLed::blink() void IndicatorLed::update() { - if (led_mode == LED_BLINK && current_state == LED_OFF) - { - digitalWrite(_pin, HIGH); - current_state = LED_ON; - } - else if (led_mode == LED_BLINK && current_state == LED_ON) - { - digitalWrite(_pin, LOW); - current_state = LED_OFF; - } - else if (led_mode == LED_ON) - { - digitalWrite(_pin, HIGH); - current_state = LED_ON; - } - else - { - digitalWrite(_pin, LOW); - current_state = LED_OFF; - } + if (led_mode == LED_BLINK && current_state == LED_OFF) + { + digitalWrite(_pin, HIGH); + current_state = LED_ON; + } + else if (led_mode == LED_BLINK && current_state == LED_ON) + { + digitalWrite(_pin, LOW); + current_state = LED_OFF; + } + else if (led_mode == LED_ON) + { + digitalWrite(_pin, HIGH); + current_state = LED_ON; + } + else + { + digitalWrite(_pin, LOW); + current_state = LED_OFF; + } } diff --git a/firmware/src/IndicatorLed.h b/firmware/src/IndicatorLed.h index dc188e5..4d7b3a4 100644 --- a/firmware/src/IndicatorLed.h +++ b/firmware/src/IndicatorLed.h @@ -36,20 +36,19 @@ const int LED_OFF = 0; const int LED_ON = 1; const int LED_BLINK = 2; -class IndicatorLed -{ - public: - IndicatorLed(int pin); - void begin(); - void on(); - void off(); - void blink(); - void update(); - - private: - int _pin; - int led_mode = LED_OFF; - int current_state = LED_OFF; +class IndicatorLed { +public: + IndicatorLed(int pin); + void begin(); + void on(); + void off(); + void blink(); + void update(); + +private: + int _pin; + int led_mode = LED_OFF; + int current_state = LED_OFF; }; -#endif \ No newline at end of file +#endif diff --git a/firmware/src/cmdr_joystick.cpp b/firmware/src/cmdr_joystick.cpp index 52fb685..67a2acf 100644 --- a/firmware/src/cmdr_joystick.cpp +++ b/firmware/src/cmdr_joystick.cpp @@ -29,12 +29,11 @@ * Joystick based on standard teensy "usb_joystick" library for HID joystick usb data communication. */ - +#include "ElrsTx.h" +#include "IndicatorLed.h" #include #include #include -#include "ElrsTx.h" -#include "IndicatorLed.h" IndicatorLed status_led(13); IndicatorLed button_led_1(22); @@ -79,33 +78,32 @@ const int DEADZONE_X = 50; const int DEADZONE_Y = 50; const int JOYSTICK_HAT_CENTER = -1; -enum EEPROM_ADR -{ - MAX_X1_ADR_HIGH, - MAX_X1_ADR_LOW, - MIN_X1_ADR_HIGH, - MIN_X1_ADR_LOW, - CNT_X1_ADR_HIGH, - CNT_X1_ADR_LOW, - MAX_Y1_ADR_HIGH, - MAX_Y1_ADR_LOW, - MIN_Y1_ADR_HIGH, - MIN_Y1_ADR_LOW, - CNT_Y1_ADR_HIGH, - CNT_Y1_ADR_LOW, - MAX_X2_ADR_HIGH, - MAX_X2_ADR_LOW, - MIN_X2_ADR_HIGH, - MIN_X2_ADR_LOW, - CNT_X2_ADR_HIGH, - CNT_X2_ADR_LOW, - MAX_Y2_ADR_HIGH, - MAX_Y2_ADR_LOW, - MIN_Y2_ADR_HIGH, - MIN_Y2_ADR_LOW, - CNT_Y2_ADR_HIGH, - CNT_Y2_ADR_LOW, - EEPROM_ADR_NBR_OF_BYTES +enum EEPROM_ADR { + MAX_X1_ADR_HIGH, + MAX_X1_ADR_LOW, + MIN_X1_ADR_HIGH, + MIN_X1_ADR_LOW, + CNT_X1_ADR_HIGH, + CNT_X1_ADR_LOW, + MAX_Y1_ADR_HIGH, + MAX_Y1_ADR_LOW, + MIN_Y1_ADR_HIGH, + MIN_Y1_ADR_LOW, + CNT_Y1_ADR_HIGH, + CNT_Y1_ADR_LOW, + MAX_X2_ADR_HIGH, + MAX_X2_ADR_LOW, + MIN_X2_ADR_HIGH, + MIN_X2_ADR_LOW, + CNT_X2_ADR_HIGH, + CNT_X2_ADR_LOW, + MAX_Y2_ADR_HIGH, + MAX_Y2_ADR_LOW, + MIN_Y2_ADR_HIGH, + MIN_Y2_ADR_LOW, + CNT_Y2_ADR_HIGH, + CNT_Y2_ADR_LOW, + EEPROM_ADR_NBR_OF_BYTES }; int joystick_counter = 0; @@ -146,69 +144,67 @@ int joystick_calibration_mode = CALIBRATION_OFF; /** * @brief Save calibration data to EEPROM */ -void save_to_eeprom() -{ - EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max); - EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8); - EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min); - EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_12bit_min >> 8); - EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_12bit_center); - EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_12bit_center >> 8); +void save_to_eeprom() { + EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max); + EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8); + EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min); + EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_12bit_min >> 8); + EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_12bit_center); + EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_12bit_center >> 8); - EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_12bit_max); - EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_12bit_max >> 8); - EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_12bit_min); - EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_12bit_min >> 8); - EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_12bit_center); - EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_12bit_center >> 8); + EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_12bit_max); + EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_12bit_max >> 8); + EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_12bit_min); + EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_12bit_min >> 8); + EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_12bit_center); + EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_12bit_center >> 8); - EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_12bit_max); - EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_12bit_max >> 8); - EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_12bit_min); - EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_12bit_min >> 8); - EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_12bit_center); - EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_12bit_center >> 8); + EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_12bit_max); + EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_12bit_max >> 8); + EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_12bit_min); + EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_12bit_min >> 8); + EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_12bit_center); + EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_12bit_center >> 8); - EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_12bit_max); - EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_12bit_max >> 8); - EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_12bit_min); - EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_12bit_min >> 8); - EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_12bit_center); - EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_12bit_center >> 8); + EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_12bit_max); + EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_12bit_max >> 8); + EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_12bit_min); + EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_12bit_min >> 8); + EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_12bit_center); + EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_12bit_center >> 8); } /** * @brief Load calibration data from EEPROM */ -void load_from_eeprom() -{ - joystick_x1_12bit_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8); - joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW); - joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8); - joystick_x1_12bit_min |= EEPROM.read(MIN_X1_ADR_LOW); - joystick_x1_12bit_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8); - joystick_x1_12bit_center |= EEPROM.read(CNT_X1_ADR_LOW); +void load_from_eeprom() { + joystick_x1_12bit_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8); + joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW); + joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8); + joystick_x1_12bit_min |= EEPROM.read(MIN_X1_ADR_LOW); + joystick_x1_12bit_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8); + joystick_x1_12bit_center |= EEPROM.read(CNT_X1_ADR_LOW); - joystick_y1_12bit_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8); - joystick_y1_12bit_max |= EEPROM.read(MAX_Y1_ADR_LOW); - joystick_y1_12bit_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8); - joystick_y1_12bit_min |= EEPROM.read(MIN_Y1_ADR_LOW); - joystick_y1_12bit_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8); - joystick_y1_12bit_center |= EEPROM.read(CNT_Y1_ADR_LOW); + joystick_y1_12bit_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8); + joystick_y1_12bit_max |= EEPROM.read(MAX_Y1_ADR_LOW); + joystick_y1_12bit_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8); + joystick_y1_12bit_min |= EEPROM.read(MIN_Y1_ADR_LOW); + joystick_y1_12bit_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8); + joystick_y1_12bit_center |= EEPROM.read(CNT_Y1_ADR_LOW); - joystick_x2_12bit_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8); - joystick_x2_12bit_max |= EEPROM.read(MAX_X2_ADR_LOW); - joystick_x2_12bit_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8); - joystick_x2_12bit_min |= EEPROM.read(MIN_X2_ADR_LOW); - joystick_x2_12bit_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8); - joystick_x2_12bit_center |= EEPROM.read(CNT_X2_ADR_LOW); + joystick_x2_12bit_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8); + joystick_x2_12bit_max |= EEPROM.read(MAX_X2_ADR_LOW); + joystick_x2_12bit_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8); + joystick_x2_12bit_min |= EEPROM.read(MIN_X2_ADR_LOW); + joystick_x2_12bit_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8); + joystick_x2_12bit_center |= EEPROM.read(CNT_X2_ADR_LOW); - joystick_y2_12bit_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8); - joystick_y2_12bit_max |= EEPROM.read(MAX_Y2_ADR_LOW); - joystick_y2_12bit_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8); - joystick_y2_12bit_min |= EEPROM.read(MIN_Y2_ADR_LOW); - joystick_y2_12bit_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8); - joystick_y2_12bit_center |= EEPROM.read(CNT_Y2_ADR_LOW); + joystick_y2_12bit_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8); + joystick_y2_12bit_max |= EEPROM.read(MAX_Y2_ADR_LOW); + joystick_y2_12bit_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8); + joystick_y2_12bit_min |= EEPROM.read(MIN_Y2_ADR_LOW); + joystick_y2_12bit_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8); + joystick_y2_12bit_center |= EEPROM.read(CNT_Y2_ADR_LOW); } /** @@ -219,166 +215,145 @@ void load_from_eeprom() * @param analog_x2 * @param analog_y2 */ -void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) -{ - joystick_x1_12bit = AXIS_12BIT_CENTER; - joystick_y1_12bit = AXIS_12BIT_CENTER; - joystick_x2_12bit = AXIS_12BIT_CENTER; - joystick_y2_12bit = AXIS_12BIT_CENTER; +void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) { + joystick_x1_12bit = AXIS_12BIT_CENTER; + joystick_y1_12bit = AXIS_12BIT_CENTER; + joystick_x2_12bit = AXIS_12BIT_CENTER; + joystick_y2_12bit = AXIS_12BIT_CENTER; - // Check for calibration mode - if (joystick_calibration_mode == CALIBRATION_INIT) - { - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED) - { - joystick_calibration_mode = CALIBRATION_CENTER; - } + // Check for calibration mode + if (joystick_calibration_mode == CALIBRATION_INIT) { + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED) { + joystick_calibration_mode = CALIBRATION_CENTER; } + } - // Calibrate joystick center values - else if (joystick_calibration_mode == CALIBRATION_CENTER) - { - joystick_x1_12bit_center = analog_x1; - joystick_y1_12bit_center = analog_y1; - joystick_x1_12bit_max = joystick_x1_12bit_center; - joystick_x1_12bit_min = joystick_x1_12bit_center; - joystick_y1_12bit_max = joystick_y1_12bit_center; - joystick_y1_12bit_min = joystick_y1_12bit_center; + // Calibrate joystick center values + else if (joystick_calibration_mode == CALIBRATION_CENTER) { + joystick_x1_12bit_center = analog_x1; + joystick_y1_12bit_center = analog_y1; + joystick_x1_12bit_max = joystick_x1_12bit_center; + joystick_x1_12bit_min = joystick_x1_12bit_center; + joystick_y1_12bit_max = joystick_y1_12bit_center; + joystick_y1_12bit_min = joystick_y1_12bit_center; - joystick_x2_12bit_center = analog_x2; - joystick_y2_12bit_center = analog_y2; - joystick_x2_12bit_max = joystick_x2_12bit_center; - joystick_x2_12bit_min = joystick_x2_12bit_center; - joystick_y2_12bit_max = joystick_y2_12bit_center; - joystick_y2_12bit_min = joystick_y2_12bit_center; + joystick_x2_12bit_center = analog_x2; + joystick_y2_12bit_center = analog_y2; + joystick_x2_12bit_max = joystick_x2_12bit_center; + joystick_x2_12bit_min = joystick_x2_12bit_center; + joystick_y2_12bit_max = joystick_y2_12bit_center; + joystick_y2_12bit_min = joystick_y2_12bit_center; - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { - joystick_calibration_mode = CALIBRATION_MINMAX; - } + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) { + joystick_calibration_mode = CALIBRATION_MINMAX; } + } - // Calibrate joystick min/max values - else if (joystick_calibration_mode == CALIBRATION_MINMAX) - { - if (analog_x1 > joystick_x1_12bit_max) joystick_x1_12bit_max = analog_x1; - if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = analog_x1; - if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1; - if (analog_y1 < joystick_y1_12bit_min) joystick_y1_12bit_min = analog_y1; - if (analog_x2 > joystick_x2_12bit_max) joystick_x2_12bit_max = analog_x2; - if (analog_x2 < joystick_x2_12bit_min) joystick_x2_12bit_min = analog_x2; - if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2; - if (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2; + // Calibrate joystick min/max values + else if (joystick_calibration_mode == CALIBRATION_MINMAX) { + if (analog_x1 > joystick_x1_12bit_max) joystick_x1_12bit_max = analog_x1; + if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = analog_x1; + if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1; + if (analog_y1 < joystick_y1_12bit_min) joystick_y1_12bit_min = analog_y1; + if (analog_x2 > joystick_x2_12bit_max) joystick_x2_12bit_max = analog_x2; + if (analog_x2 < joystick_x2_12bit_min) joystick_x2_12bit_min = analog_x2; + if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2; + if (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2; - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) - { - joystick_calibration_mode = CALIBRATION_OFF; - save_to_eeprom(); - } + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { + joystick_calibration_mode = CALIBRATION_OFF; + save_to_eeprom(); } + } } /** * @brief Save joystick calibration values to EEPROM */ -void send_elrs_data() -{ - // Set ELRS analog channels - elrs.set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0); - elrs.set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1); - elrs.set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2); - elrs.set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3); +void send_elrs_data() { + // Set ELRS analog channels + elrs.set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0); + elrs.set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1); + elrs.set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2); + elrs.set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3); - // Set ELRS digital channels - for (int i = 4; i < CRSF_MAX_CHANNEL; i++) - { - elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i); - } + // Set ELRS digital channels + for (int i = 4; i < CRSF_MAX_CHANNEL; i++) { + elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i); + } - if (toggle_button_arm) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 4); - if (toggle_button_mode) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 5); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 6); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 7); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8); + if (toggle_button_arm) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 4); + if (toggle_button_mode) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 5); + if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 6); + if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 7); + if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8); - // Send ELRS data - elrs.send_data(); + // Send ELRS data + elrs.send_data(); } /** * Send USB data to PC */ -void send_usb_data() -{ - // Set USB analog channels - int joystick_x1_10bit = map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); - int joystick_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); - int joystick_x2_10bit = map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); - int joystick_y2_10bit = map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); +void send_usb_data() { + // Set USB analog channels + int joystick_x1_10bit = map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); + int joystick_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); + int joystick_x2_10bit = map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); + int joystick_y2_10bit = map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX); - Joystick.Zrotate(joystick_x1_10bit); - Joystick.Z(joystick_y1_10bit); + Joystick.Zrotate(joystick_x1_10bit); + Joystick.Z(joystick_y1_10bit); - if (fn_mode == 2) - { - Joystick.X(AXIS_10BIT_CENTER); - Joystick.Y(AXIS_10BIT_CENTER); - Joystick.sliderRight(joystick_x2_10bit); - Joystick.sliderLeft(joystick_y2_10bit); - } - else if (fn_mode == 1) - { - Joystick.X(AXIS_10BIT_CENTER); - Joystick.Y(joystick_y2_10bit); - Joystick.sliderRight(joystick_x2_10bit); - Joystick.sliderLeft(AXIS_10BIT_CENTER); - } - else - { - Joystick.X(joystick_x2_10bit); - Joystick.Y(joystick_y2_10bit); - Joystick.sliderRight(AXIS_10BIT_CENTER); - Joystick.sliderLeft(AXIS_10BIT_CENTER); - } + if (fn_mode == 2) { + Joystick.X(AXIS_10BIT_CENTER); + Joystick.Y(AXIS_10BIT_CENTER); + Joystick.sliderRight(joystick_x2_10bit); + Joystick.sliderLeft(joystick_y2_10bit); + } else if (fn_mode == 1) { + Joystick.X(AXIS_10BIT_CENTER); + Joystick.Y(joystick_y2_10bit); + Joystick.sliderRight(joystick_x2_10bit); + Joystick.sliderLeft(AXIS_10BIT_CENTER); + } else { + Joystick.X(joystick_x2_10bit); + Joystick.Y(joystick_y2_10bit); + Joystick.sliderRight(AXIS_10BIT_CENTER); + Joystick.sliderLeft(AXIS_10BIT_CENTER); + } - // Set USB digital channels - for (int i = 1; i < 32; i++) - { - Joystick.button(i, 0); - } + // Set USB digital channels + for (int i = 1; i < 32; i++) { + Joystick.button(i, 0); + } - Joystick.hat(JOYSTICK_HAT_CENTER); + Joystick.hat(JOYSTICK_HAT_CENTER); - if (fn_mode == 2) - { - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1); - } - else if (fn_mode == 1) - { - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); - } - else - { - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1); - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); - } + if (fn_mode == 2) { + if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1); + if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1); + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1); + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1); + } else if (fn_mode == 1) { + if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1); + if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1); + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1); + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); + } else { + if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1); + if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1); + if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1); + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1); + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); + } - Joystick.send_now(); + Joystick.send_now(); } /** @@ -391,225 +366,189 @@ void send_usb_data() * @param deadband_value * @param expo_value */ -int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int center_value, int deadband_value, int expo_value) -{ - int calibrated_value = AXIS_12BIT_CENTER; +int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int center_value, int deadband_value, int expo_value) { + int calibrated_value = AXIS_12BIT_CENTER; - if (gimbal_value > (center_value + deadband_value)) - { - calibrated_value = constrain(map(gimbal_value, (center_value + deadband_value), max_value, AXIS_12BIT_CENTER, AXIS_12BIT_MAX), AXIS_12BIT_CENTER, AXIS_12BIT_MAX); - } - else if (gimbal_value < (center_value - deadband_value)) - { - calibrated_value = constrain(map(gimbal_value, min_value, (center_value - deadband_value), AXIS_12BIT_MIN, AXIS_12BIT_CENTER), AXIS_12BIT_MIN, AXIS_12BIT_CENTER); - } + if (gimbal_value > (center_value + deadband_value)) { + calibrated_value = constrain(map(gimbal_value, (center_value + deadband_value), max_value, AXIS_12BIT_CENTER, AXIS_12BIT_MAX), AXIS_12BIT_CENTER, AXIS_12BIT_MAX); + } else if (gimbal_value < (center_value - deadband_value)) { + calibrated_value = constrain(map(gimbal_value, min_value, (center_value - deadband_value), AXIS_12BIT_MIN, AXIS_12BIT_CENTER), AXIS_12BIT_MIN, AXIS_12BIT_CENTER); + } - if (expo_value != 0) - { - float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX); - /* Calculate expo using 9th order polynomial function with 0.5 as center point */ - float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float; - calibrated_value = constrain(int(joystick_x_exp * float(AXIS_12BIT_MAX)), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - } + if (expo_value != 0) { + float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX); + /* Calculate expo using 9th order polynomial function with 0.5 as center point */ + float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float; + calibrated_value = constrain(int(joystick_x_exp * float(AXIS_12BIT_MAX)), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + } - return calibrated_value; + return calibrated_value; } /** * @brief Process input data from gimbal and buttons */ -void process_input_data() -{ - int analog_x1_gimbal_value = 0; - int analog_y1_gimbal_value = 0; - int analog_x2_gimbal_value = 0; - int analog_y2_gimbal_value = 0; +void process_input_data() { + int analog_x1_gimbal_value = 0; + int analog_y1_gimbal_value = 0; + int analog_x2_gimbal_value = 0; + int analog_y2_gimbal_value = 0; - if (gimbal_mode == GIMBAL_MODE_FRSKY_M10) - { - analog_x1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_y1_gimbal_value = constrain(analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_x2_gimbal_value = constrain(analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_y2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - } - else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7) - { - analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_y1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_x2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); - } + if (gimbal_mode == GIMBAL_MODE_FRSKY_M10) { + analog_x1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_y1_gimbal_value = constrain(analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_x2_gimbal_value = constrain(analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_y2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + } else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7) { + analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_y1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_x2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); + } - if (joystick_calibration_mode != CALIBRATION_OFF) - { - calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value); - return; - } + if (joystick_calibration_mode != CALIBRATION_OFF) { + calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value); + return; + } - joystick_x1_12bit = apply_calibration_12bit(analog_x1_gimbal_value, joystick_x1_12bit_min, joystick_x1_12bit_max, joystick_x1_12bit_center, DEADZONE_X, exp_constant); - joystick_y1_12bit = apply_calibration_12bit(analog_y1_gimbal_value, joystick_y1_12bit_min, joystick_y1_12bit_max, joystick_y1_12bit_center, DEADZONE_Y, 0); - joystick_x2_12bit = apply_calibration_12bit(analog_x2_gimbal_value, joystick_x2_12bit_min, joystick_x2_12bit_max, joystick_x2_12bit_center, DEADZONE_X, exp_constant); - joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max, joystick_y2_12bit_center, DEADZONE_Y, exp_constant); + joystick_x1_12bit = apply_calibration_12bit(analog_x1_gimbal_value, joystick_x1_12bit_min, joystick_x1_12bit_max, joystick_x1_12bit_center, DEADZONE_X, exp_constant); + joystick_y1_12bit = apply_calibration_12bit(analog_y1_gimbal_value, joystick_y1_12bit_min, joystick_y1_12bit_max, joystick_y1_12bit_center, DEADZONE_Y, 0); + joystick_x2_12bit = apply_calibration_12bit(analog_x2_gimbal_value, joystick_x2_12bit_min, joystick_x2_12bit_max, joystick_x2_12bit_center, DEADZONE_X, exp_constant); + joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max, joystick_y2_12bit_center, DEADZONE_Y, exp_constant); - // Check fn mode - fn_mode = 0; - if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { - fn_mode = 1; - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) - { - fn_mode = 2; - } + // Check fn mode + fn_mode = 0; + if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { + fn_mode = 1; + if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { + fn_mode = 2; } + } - // Check toggle mode buttons - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) - { - toggle_button_mode = true; - } - else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { - toggle_button_mode = false; - } + // Check toggle mode buttons + if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { + toggle_button_mode = true; + } else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { + toggle_button_mode = false; + } - // Check toggle arm button - if ((fn_mode == 1) && (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) != toggle_button_arm_previous_value)) - { - if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == false)) - { - toggle_button_arm = true; - } - else if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == true)) - { - toggle_button_arm = false; - } - toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN); + // Check toggle arm button + if ((fn_mode == 1) && (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) != toggle_button_arm_previous_value)) { + if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == false)) { + toggle_button_arm = true; + } else if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == true)) { + toggle_button_arm = false; } + toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN); + } } -void setup() -{ - /* Init HW */ - status_led.begin(); - status_led.blink(); - button_led_1.begin(); - button_led_2.begin(); +void setup() { + /* Init HW */ + status_led.begin(); + status_led.blink(); + button_led_1.begin(); + button_led_2.begin(); - pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP); - pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP); - pinMode(BUTTON_FRONT_RIGHT_LOWER_PIN, INPUT_PULLUP); - pinMode(BUTTON_FRONT_RIGHT_UPPER_PIN, INPUT_PULLUP); - pinMode(BUTTON_TOP_LEFT_LOWER_PIN, INPUT_PULLUP); - pinMode(BUTTON_TOP_RIGHT_LOWER_PIN, INPUT_PULLUP); - pinMode(BUTTON_TOP_LEFT_UPPER_PIN, INPUT_PULLUP); - pinMode(BUTTON_TOP_RIGHT_UPPER_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRONT_RIGHT_LOWER_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRONT_RIGHT_UPPER_PIN, INPUT_PULLUP); + pinMode(BUTTON_TOP_LEFT_LOWER_PIN, INPUT_PULLUP); + pinMode(BUTTON_TOP_RIGHT_LOWER_PIN, INPUT_PULLUP); + pinMode(BUTTON_TOP_LEFT_UPPER_PIN, INPUT_PULLUP); + pinMode(BUTTON_TOP_RIGHT_UPPER_PIN, INPUT_PULLUP); - pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP); + pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP); - // Set ADC resolution to 12bit - analogReadResolution(12); - analogReadAveraging(32); - delay(500); + // Set ADC resolution to 12bit + analogReadResolution(12); + analogReadAveraging(32); + delay(500); - // Set analog (lib) resolution to 12bit - analog_x1.setAnalogResolution(4096); - analog_y1.setAnalogResolution(4096); - analog_x2.setAnalogResolution(4096); - analog_y2.setAnalogResolution(4096); + // Set analog (lib) resolution to 12bit + analog_x1.setAnalogResolution(4096); + analog_y1.setAnalogResolution(4096); + analog_x2.setAnalogResolution(4096); + analog_y2.setAnalogResolution(4096); - // Init EEPROM - load_from_eeprom(); + // Init EEPROM + load_from_eeprom(); - // Init Joystick - Joystick.useManualSend(true); + // Init Joystick + Joystick.useManualSend(true); - // Check if calibration mode is enabled - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { - joystick_calibration_mode = CALIBRATION_INIT; - } + // Check if calibration mode is enabled + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) { + joystick_calibration_mode = CALIBRATION_INIT; + } - // Check if bootloader mode is enabled - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) - { - button_led_2.on(); - button_led_2.update(); - delay(200); - _reboot_Teensyduino_(); - } + // Check if bootloader mode is enabled + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { + button_led_2.on(); + button_led_2.update(); + delay(200); + _reboot_Teensyduino_(); + } - // Check what gimbal mode is selected - if (digitalRead(GIMBAL_MODE_PIN) == LOW) - { - gimbal_mode = GIMBAL_MODE_FRSKY_M10; - } + // Check what gimbal mode is selected + if (digitalRead(GIMBAL_MODE_PIN) == LOW) { + gimbal_mode = GIMBAL_MODE_FRSKY_M10; + } - // Init ELRS - elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW); + // Init ELRS + elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW); } -void loop() -{ - current_timestamp_micros = micros(); +void loop() { + current_timestamp_micros = micros(); - analog_x1.update(); - analog_y1.update(); - analog_x2.update(); - analog_y2.update(); + analog_x1.update(); + analog_y1.update(); + analog_x2.update(); + analog_y2.update(); - /* Process data with 1ms interval*/ - if (current_timestamp_micros >= process_data_timestamp_micros) - { - process_input_data(); - process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms; + /* Process data with 1ms interval*/ + if (current_timestamp_micros >= process_data_timestamp_micros) { + process_input_data(); + process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms; + } + + /* Update/Send USB data with 5ms interval*/ + if (current_timestamp_micros >= send_usb_timestamp_micros) { + send_usb_data(); + send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms; + } + + /* Update/Send ERLS data with about 1,6ms interval */ + if (current_timestamp_micros >= send_elrs_timestamp_micros) { + send_elrs_data(); + send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; + } + + /* Update indicator with 200ms interval */ + if (current_timestamp_micros >= indicator_timestamp_micros) { + button_led_1.off(); + button_led_2.off(); + + if (joystick_calibration_mode == CALIBRATION_INIT) { + button_led_1.blink(); + button_led_2.blink(); + } else if (joystick_calibration_mode == CALIBRATION_CENTER) { + button_led_1.blink(); + button_led_2.off(); + } else if (joystick_calibration_mode == CALIBRATION_MINMAX) { + button_led_1.off(); + button_led_2.blink(); + } else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) || (joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER)) { + button_led_1.on(); + button_led_2.on(); } - /* Update/Send USB data with 5ms interval*/ - if (current_timestamp_micros >= send_usb_timestamp_micros) - { - send_usb_data(); - send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms; - } + status_led.update(); + button_led_1.update(); + button_led_2.update(); - /* Update/Send ERLS data with about 1,6ms interval */ - if (current_timestamp_micros >= send_elrs_timestamp_micros) - { - send_elrs_data(); - send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; - } - - /* Update indicator with 200ms interval */ - if (current_timestamp_micros >= indicator_timestamp_micros) - { - button_led_1.off(); - button_led_2.off(); - - if (joystick_calibration_mode == CALIBRATION_INIT) - { - button_led_1.blink(); - button_led_2.blink(); - } - else if (joystick_calibration_mode == CALIBRATION_CENTER) - { - button_led_1.blink(); - button_led_2.off(); - } - else if (joystick_calibration_mode == CALIBRATION_MINMAX) - { - button_led_1.off(); - button_led_2.blink(); - } - else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) || - (joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER)) - { - button_led_1.on(); - button_led_2.on(); - } - - status_led.update(); - button_led_1.update(); - button_led_2.update(); - - indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms; - } + indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms; + } }