From 727f113124dce209ac8071202a65c3b006fc2ba5 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 22 May 2023 23:25:56 +0000 Subject: [PATCH] Changed to OOP coding --- .gitignore | 1 + firmware/src/{erls.cpp => ElrsTx.cpp} | 192 ++++++++++++++---------- firmware/src/ElrsTx.h | 119 +++++++++++++++ firmware/src/IndicatorLed.cpp | 101 +++++++++++++ firmware/src/{erls.h => IndicatorLed.h} | 56 +++---- firmware/src/cmdr_joystick.cpp | 164 ++++++++------------ 6 files changed, 414 insertions(+), 219 deletions(-) create mode 100644 .gitignore rename firmware/src/{erls.cpp => ElrsTx.cpp} (52%) create mode 100644 firmware/src/ElrsTx.h create mode 100644 firmware/src/IndicatorLed.cpp rename firmware/src/{erls.h => IndicatorLed.h} (60%) diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7f6dc73 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +firmware/.cache/clangd/index diff --git a/firmware/src/erls.cpp b/firmware/src/ElrsTx.cpp similarity index 52% rename from firmware/src/erls.cpp rename to firmware/src/ElrsTx.cpp index 85217c3..4694ed1 100644 --- a/firmware/src/erls.cpp +++ b/firmware/src/ElrsTx.cpp @@ -27,127 +27,153 @@ * A PARTICULAR PURPOSE. See the GNU General Public License for more details. */ -#include "erls.h" +#include "ElrsTx.h" -uint8_t crsf_packet[CRSF_PACKET_SIZE]; -uint8_t crsf_cmd_packet[CRSF_CMD_PACKET_SIZE]; -int16_t elrs_channels[CRSF_MAX_CHANNEL]; +/** + * @brief Construct a new ElrsTx object + * + * @param serial The serial port to use + */ +ElrsTx::ElrsTx(HardwareSerial& serial): _serial(serial) { } -bool elrs_init_done = false; -int elrs_init_counter = 0; +/** + * @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); -// 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}; + _packet_rate = packet_rate; + _tx_power = tx_power; +} /** * @brief Calculate the CRC8 of a CRSF packet * * @param ptr The packet to be calculated * @param len The length of the packet + * @return The CRC8 of the packet */ -uint8_t calculate_crsf_crc8(const uint8_t *ptr, uint8_t len) +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++]; + crc = CRSF_CRC8TAB[crc ^ *ptr++]; } return crc; } /** - * @brief Prepare a CRSF data packet + * @brief Prepare a normal CRSF data packet * - * @param packet The packet to be prepared - * @param channels The channels to be sent + * packet format: + * 0 - header + * 1 - length + * 2 - type + * 3-24 - data (16 11bit channels in 8bit format) + * 25 - crc + * + * @param packet Resulting packet + * @param channels Channel data */ -void prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) +void ElrsTx::prepare_crsf_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); + 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); // 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] = calculate_crsf_crc8(&packet[2], packet[1] - 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); } /** * @brief Prepare CRSF command packet * - * @param packet_cmd - * @param command - * @param value + * packet format: + * 0 - header + * 1 - length + * 2 - type + * 3 - address + * 4 - address radio + * 5 - command + * 6 - value + * 7 - crc + * + * 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 value Value to be sent */ -void prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) +void ElrsTx::prepare_crsf_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] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); + 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); } /** - * @brief Set data to be sent to ERLS TX + * @brief Set ELRS channel data * - * @param value - * @param channel + * @param value Value (0-2048) to be set + * @param channel Channel (0-11) to be set */ -void erls_set_data(int16_t value, uint8_t channel) +void ElrsTx::set_data(int16_t value, uint8_t channel) { elrs_channels[channel] = value; } /** - * @brief Send data to ERLS TX - * + * @brief Send data to ERLS TX. This function should be called within the main loop with a 1.6ms interval */ -void erls_send_data() +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); - ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE); + _serial.write(crsf_packet, CRSF_PACKET_SIZE); } else { @@ -155,21 +181,21 @@ void erls_send_data() if (elrs_init_counter < 500) { prepare_crsf_data_packet(crsf_packet, elrs_channels); - ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE); + _serial.write(crsf_packet, CRSF_PACKET_SIZE); elrs_init_counter++; } - // Send command to update TX packet rate to 500Hz + // Send command to update TX packet rate else if (elrs_init_counter < 505) { - prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE); - ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + 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 to 100mW + // Send command to update TX power else if (elrs_init_counter < 510) { - prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER); - ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + 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 diff --git a/firmware/src/ElrsTx.h b/firmware/src/ElrsTx.h new file mode 100644 index 0000000..65b2a50 --- /dev/null +++ b/firmware/src/ElrsTx.h @@ -0,0 +1,119 @@ + /* + * ======================================================================================================= + * ------------------------------------------------------------------------------------------------------- + * ---####################-----###########-------###########-----############--############-############-- + * --######################---#############-----#############---- -- - --- + * --###### ##---##### ###-----### #####---------##-------#######------#------------- + * -- -------------- --- ----- --- ----- ---------##-------#------------#------------- + * --#####--------------------#####------####-####------#####---------##-------###########--############-- + * -- -------------------- ------ ------ --------- ------- -- -- + * --#####--------------------#####--------#####--------#####--------------------------------------------- + * -- -------------------- -------- -------- --------------------------------------------- + * --######--------------##---#####---------------------#####---------------- ERLS ----------------------- + * --##################### ---#####---------------------#####--------------------------------------------- + * ---################### ----#####---------------------#####--------------------------------------------- + * --- ----- --------------------- --------------------------------------------- + * ------------------------------------------------------------------------------------------------------- + * ======================================================================================================= + * + * Copyright 2023 Christoffer Martinsson + * + * CMtec ERLS can be redistributed and/or modified under the terms of the GNU General + * Public License (Version 2), as published by the Free Software Foundation. + * A copy of the license can be found online at www.gnu.o urg/licenses. + * + * CMtec ERLS is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR + * A PARTICULAR PURPOSE. See the GNU General Public License for more details. + */ + +#ifndef ELRSTX_H +#define ELRSTX_H + +#include + +// 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_MAX = 1811; +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_SERIAL_BAUDRATE = 400000; + +// ELRS command +const int ELRS_ADDRESS = 0xEE; +const int ELRS_BIND_COMMAND = 0xFF; +const int ELRS_WIFI_COMMAND = 0xFE; +const int ELRS_PKT_RATE_COMMAND = 0x01; +const int ELRS_TLM_RATIO_COMMAND = 0x02; +const int ELRS_SWITCH_MODE_COMMAND = 0x03; +const int ELRS_MODEL_MATCH_COMMAND = 0x04; +const int ELRS_POWER_COMMAND = 0x06; +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_TX_POWER_10mW = 0; +const int ELRS_TX_POWER_25mW = 1; +const int ELRS_TX_POWER_100mW = 2; +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]; + + 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 diff --git a/firmware/src/IndicatorLed.cpp b/firmware/src/IndicatorLed.cpp new file mode 100644 index 0000000..3001d73 --- /dev/null +++ b/firmware/src/IndicatorLed.cpp @@ -0,0 +1,101 @@ +/* + * ======================================================================================================= + * ------------------------------------------------------------------------------------------------------- + * ---####################-----###########-------###########-----############--############-############-- + * --######################---#############-----#############---- -- - --- + * --###### ##---##### ###-----### #####---------##-------#######------#------------- + * -- -------------- --- ----- --- ----- ---------##-------#------------#------------- + * --#####--------------------#####------####-####------#####---------##-------###########--############-- + * -- -------------------- ------ ------ --------- ------- -- -- + * --#####--------------------#####--------#####--------#####--------------------------------------------- + * -- -------------------- -------- -------- --------------------------------------------- + * --######--------------##---#####---------------------#####---------------- IndicatorLed --------------- + * --##################### ---#####---------------------#####--------------------------------------------- + * ---################### ----#####---------------------#####--------------------------------------------- + * --- ----- --------------------- --------------------------------------------- + * ------------------------------------------------------------------------------------------------------- + * ======================================================================================================= + * + * Copyright 2023 Christoffer Martinsson + * + * CMtec ERLS can be redistributed and/or modified under the terms of the GNU General + * Public License (Version 2), as published by the Free Software Foundation. + * A copy of the license can be found online at www.gnu.o urg/licenses. + * + * CMtec ERLS is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR + * A PARTICULAR PURPOSE. See the GNU General Public License for more details. + */ + +#include "IndicatorLed.h" + +/** + * @brief Construct a new Indicator Led:: Indicator Led object + * + * @param pin Pin number for indicator LED + */ +IndicatorLed::IndicatorLed(int pin) +{ + _pin = pin; +} + +/** + * @brief Initialize indicator LED + */ +void IndicatorLed::begin() +{ + pinMode(_pin, OUTPUT); + digitalWrite(_pin, LOW); +} + +/** + * @brief Turn on indicator LED + */ +void IndicatorLed::on() +{ + led_mode = LED_ON; +} + +/** + * @brief Turn off indicator LED + */ +void IndicatorLed::off() +{ + led_mode = LED_OFF; +} + +/** + * @brief Blink indicator LED + */ +void IndicatorLed::blink() +{ + led_mode = LED_BLINK; +} + +/** + * @brief Update indicator LED. Call this function in the main loop with a 200-500 ms interval. + */ +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; + } +} diff --git a/firmware/src/erls.h b/firmware/src/IndicatorLed.h similarity index 60% rename from firmware/src/erls.h rename to firmware/src/IndicatorLed.h index e09c85f..dc188e5 100644 --- a/firmware/src/erls.h +++ b/firmware/src/IndicatorLed.h @@ -1,4 +1,4 @@ - /* +/* * ======================================================================================================= * ------------------------------------------------------------------------------------------------------- * ---####################-----###########-------###########-----############--############-############-- @@ -9,7 +9,7 @@ * -- -------------------- ------ ------ --------- ------- -- -- * --#####--------------------#####--------#####--------#####--------------------------------------------- * -- -------------------- -------- -------- --------------------------------------------- - * --######--------------##---#####---------------------#####---------------- ERLS ----------------------- + * --######--------------##---#####---------------------#####---------------- IndicatorLed --------------- * --##################### ---#####---------------------#####--------------------------------------------- * ---################### ----#####---------------------#####--------------------------------------------- * --- ----- --------------------- --------------------------------------------- @@ -27,37 +27,29 @@ * A PARTICULAR PURPOSE. See the GNU General Public License for more details. */ +#ifndef INDICATORLED_H +#define INDICATORLED_H + #include -// 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 +const int LED_OFF = 0; +const int LED_ON = 1; +const int LED_BLINK = 2; -// 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 +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; +}; - -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); -void erls_set_data(int16_t value, uint8_t channel); -void erls_send_data(); \ No newline at end of file +#endif \ No newline at end of file diff --git a/firmware/src/cmdr_joystick.cpp b/firmware/src/cmdr_joystick.cpp index 07600e8..8efe249 100644 --- a/firmware/src/cmdr_joystick.cpp +++ b/firmware/src/cmdr_joystick.cpp @@ -32,38 +32,25 @@ #include #include -#include "erls.h" +#include "ElrsTx.h" +#include "IndicatorLed.h" #include "ResponsiveAnalogRead.h" -#define LED_MODE_OFF 0 -#define LED_MODE_ON 1 -#define LED_MODE_BLINK 2 -#define LED_ON HIGH -#define LED_OFF LOW +IndicatorLed status_led(13); +IndicatorLed button_led_1(22); +IndicatorLed button_led_2(23); -const int STATUS_LED_PIN = 13; -bool status_led_on = false; -int status_led_mode = LED_MODE_BLINK; - -const int BUTTON_LED_1_PIN = 22; -bool button_led_1_on = false; -int button_led_1_mode = LED_MODE_OFF; - -const int BUTTON_LED_2_PIN = 23; -bool button_led_2_on = false; -int button_led_2_mode = LED_MODE_OFF; - -#define TIME_US_1ms 1000 -#define TIME_US_5ms 5000 -#define TIME_US_200ms 200000 +const int TIME_US_1ms = 1000; +const int TIME_US_5ms = 5000; +const long TIME_US_200ms = 200000; unsigned long current_timestamp_micros = 0; unsigned long process_data_timestamp_micros = 0; unsigned long indicator_timestamp_micros = 0; unsigned long send_usb_timestamp_micros = 0; unsigned long send_elrs_timestamp_micros = 0; -#define BUTTON_PRESSED LOW -#define BUTTON_NOT_PRESSED HIGH +const int BUTTON_PRESSED = LOW; +const int BUTTON_NOT_PRESSED = HIGH; const int BUTTON_FRONT_LEFT_UPPER_PIN = 4; const int BUTTON_FRONT_LEFT_LOWER_PIN = 5; const int BUTTON_FRONT_RIGHT_UPPER_PIN = 6; @@ -73,8 +60,8 @@ const int BUTTON_TOP_RIGHT_UPPER_PIN = 10; const int BUTTON_TOP_LEFT_LOWER_PIN = 8; const int BUTTON_TOP_RIGHT_LOWER_PIN = 11; -#define GIMBAL_MODE_FRSKY_M7 0 -#define GIMBAL_MODE_FRSKY_M10 1 +const int GIMBAL_MODE_FRSKY_M7 = 0; +const int GIMBAL_MODE_FRSKY_M10 = 1; const int GIMBAL_MODE_PIN = 21; int gimbal_mode = GIMBAL_MODE_FRSKY_M7; @@ -82,15 +69,15 @@ bool toggle_button_arm = false; bool toggle_button_mode = false; int toggle_button_arm_previous_value = HIGH; -#define AXIS_10BIT_MAX 1023 -#define AXIS_10BIT_MIN 0 -#define AXIS_10BIT_CENTER 512 -#define AXIS_12BIT_MAX 4096 -#define AXIS_12BIT_MIN 0 -#define AXIS_12BIT_CENTER 2048 -#define DEADZONE_X 50 -#define DEADZONE_Y 50 -#define JOYSTICK_HAT_CENTER -1 +const int AXIS_10BIT_MAX = 1023; +const int AXIS_10BIT_MIN = 0; +const int AXIS_10BIT_CENTER = 512; +const int AXIS_12BIT_MAX = 4096; +const int AXIS_12BIT_MIN = 0; +const int AXIS_12BIT_CENTER = 2048; +const int DEADZONE_X = 50; +const int DEADZONE_Y = 50; +const int JOYSTICK_HAT_CENTER = -1; enum EEPROM_ADR { @@ -148,10 +135,12 @@ ResponsiveAnalogRead analog_y1(A1, true); ResponsiveAnalogRead analog_x2(A3, true); ResponsiveAnalogRead analog_y2(A2, true); -#define CALIBRATION_OFF 0 -#define CALIBRATION_INIT 1 -#define CALIBRATION_CENTER 2 -#define CALIBRATION_MINMAX 3 +ElrsTx elrs(Serial1); + +const int CALIBRATION_OFF = 0; +const int CALIBRATION_INIT = 1; +const int CALIBRATION_CENTER = 2; +const int CALIBRATION_MINMAX = 3; int joystick_calibration_mode = CALIBRATION_OFF; /** @@ -232,7 +221,6 @@ void load_from_eeprom() */ 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; @@ -293,29 +281,28 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) /** * @brief Save joystick calibration values to EEPROM */ -void send_erls_data() +void send_elrs_data() { - // Set ELRS analog channels - erls_set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0); - erls_set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1); - erls_set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2); - erls_set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3); + 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++) { - erls_set_data(CRSF_DIGITAL_CHANNEL_MIN, i); + elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i); } - if (toggle_button_arm) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 4); - if (toggle_button_mode) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 5); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 6); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 7); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) erls_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 - erls_send_data(); + elrs.send_data(); } /** @@ -502,45 +489,13 @@ void process_input_data() } } -/** - * @brief Update indicator LED - */ -void update_indicator(int led_mode, bool ¤t_led_state, int led_pin) -{ - - if (led_mode == LED_MODE_BLINK && current_led_state == false) - { - digitalWrite(led_pin, LED_ON); - current_led_state = true; - } - else if (led_mode == LED_MODE_BLINK && current_led_state == true) - { - digitalWrite(led_pin, LED_OFF); - current_led_state = false; - } - else if (led_mode == LED_MODE_ON) - { - digitalWrite(led_pin, LED_ON); - current_led_state = true; - } - else - { - digitalWrite(led_pin, LED_OFF); - current_led_state = false; - } -} - void setup() { /* Init HW */ - pinMode(STATUS_LED_PIN, OUTPUT); - digitalWrite(STATUS_LED_PIN, LED_OFF); - - pinMode(BUTTON_LED_1_PIN, OUTPUT); - digitalWrite(BUTTON_LED_1_PIN, LED_OFF); - - pinMode(BUTTON_LED_2_PIN, OUTPUT); - digitalWrite(BUTTON_LED_2_PIN, LED_OFF); + 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); @@ -579,7 +534,8 @@ void setup() // Check if bootloader mode is enabled if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { - digitalWrite(BUTTON_LED_2_PIN, HIGH); + button_led_2.on(); + button_led_2.update(); delay(200); _reboot_Teensyduino_(); } @@ -591,7 +547,7 @@ void setup() } // Init ELRS - ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE); + elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW); } void loop() @@ -620,41 +576,41 @@ void loop() /* Update/Send ERLS data with about 1,6ms interval */ if (current_timestamp_micros >= send_elrs_timestamp_micros) { - send_erls_data(); + 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_mode = LED_MODE_OFF; - button_led_2_mode = LED_MODE_OFF; + button_led_1.off(); + button_led_2.off(); if (joystick_calibration_mode == CALIBRATION_INIT) { - button_led_1_mode = LED_MODE_BLINK; - button_led_2_mode = LED_MODE_BLINK; + button_led_1.blink(); + button_led_2.blink(); } else if (joystick_calibration_mode == CALIBRATION_CENTER) { - button_led_1_mode = LED_MODE_BLINK; - button_led_2_mode = LED_MODE_OFF; + button_led_1.blink(); + button_led_2.off(); } else if (joystick_calibration_mode == CALIBRATION_MINMAX) { - button_led_1_mode = LED_MODE_OFF; - button_led_2_mode = LED_MODE_BLINK; + 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_mode = LED_MODE_ON; - button_led_2_mode = LED_MODE_ON; + button_led_1.on(); + button_led_2.on(); } - update_indicator(button_led_1_mode, button_led_1_on, BUTTON_LED_1_PIN); - update_indicator(button_led_2_mode, button_led_2_on, BUTTON_LED_2_PIN); - update_indicator(status_led_mode, status_led_on, STATUS_LED_PIN); + status_led.update(); + button_led_1.update(); + button_led_2.update(); indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms; }