cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Changed to OOP coding

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-05-22 23:25:56 +00:00
parent 096338398f
commit 727f113124
6 changed files with 414 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
firmware/.cache/clangd/index

192
firmware/src/erls.cpp → firmware/src/ElrsTx.cpp
View File

@ -27,127 +27,153 @@
* A PARTICULAR PURPOSE. See the GNU General Public License for more details. * 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]; * @brief Construct a new ElrsTx object
int16_t elrs_channels[CRSF_MAX_CHANNEL]; *
* @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 _packet_rate = packet_rate;
static uint8_t crsf_crc8tab[256] = { _tx_power = tx_power;
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};
/** /**
* @brief Calculate the CRC8 of a CRSF packet * @brief Calculate the CRC8 of a CRSF packet
* *
* @param ptr The packet to be calculated * @param ptr The packet to be calculated
* @param len The length of the packet * @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; uint8_t crc = 0;
for (uint8_t i = 0; i < len; i++) for (uint8_t i = 0; i < len; i++)
{ {
crc = crsf_crc8tab[crc ^ *ptr++]; crc = CRSF_CRC8TAB[crc ^ *ptr++];
} }
return crc; return crc;
} }
/** /**
* @brief Prepare a CRSF data packet * @brief Prepare a normal CRSF data packet
* *
* @param packet The packet to be prepared * packet format:
* @param channels The channels to be sent * 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 const int CRSF_TYPE_CHANNELS = 0x16;
packet[1] = 24; // length
packet[2] = CRSF_TYPE_CHANNELS; packet[0] = ELRS_ADDRESS;
packet[3] = (uint8_t)(channels[0] & 0x07FF); packet[1] = 24;
packet[4] = (uint8_t)((channels[0] & 0x07FF) >> 8 | (channels[1] & 0x07FF) << 3); packet[2] = CRSF_TYPE_CHANNELS;
packet[5] = (uint8_t)((channels[1] & 0x07FF) >> 5 | (channels[2] & 0x07FF) << 6); packet[3] = (uint8_t)(channels[0] & 0x07FF);
packet[6] = (uint8_t)((channels[2] & 0x07FF) >> 2); packet[4] = (uint8_t)((channels[0] & 0x07FF) >> 8 | (channels[1] & 0x07FF) << 3);
packet[7] = (uint8_t)((channels[2] & 0x07FF) >> 10 | (channels[3] & 0x07FF) << 1); packet[5] = (uint8_t)((channels[1] & 0x07FF) >> 5 | (channels[2] & 0x07FF) << 6);
packet[8] = (uint8_t)((channels[3] & 0x07FF) >> 7 | (channels[4] & 0x07FF) << 4); packet[6] = (uint8_t)((channels[2] & 0x07FF) >> 2);
packet[9] = (uint8_t)((channels[4] & 0x07FF) >> 4 | (channels[5] & 0x07FF) << 7); packet[7] = (uint8_t)((channels[2] & 0x07FF) >> 10 | (channels[3] & 0x07FF) << 1);
packet[10] = (uint8_t)((channels[5] & 0x07FF) >> 1); packet[8] = (uint8_t)((channels[3] & 0x07FF) >> 7 | (channels[4] & 0x07FF) << 4);
packet[11] = (uint8_t)((channels[5] & 0x07FF) >> 9 | (channels[6] & 0x07FF) << 2); packet[9] = (uint8_t)((channels[4] & 0x07FF) >> 4 | (channels[5] & 0x07FF) << 7);
packet[12] = (uint8_t)((channels[6] & 0x07FF) >> 6 | (channels[7] & 0x07FF) << 5); packet[10] = (uint8_t)((channels[5] & 0x07FF) >> 1);
packet[13] = (uint8_t)((channels[7] & 0x07FF) >> 3); packet[11] = (uint8_t)((channels[5] & 0x07FF) >> 9 | (channels[6] & 0x07FF) << 2);
packet[14] = (uint8_t)((channels[8] & 0x07FF)); packet[12] = (uint8_t)((channels[6] & 0x07FF) >> 6 | (channels[7] & 0x07FF) << 5);
packet[15] = (uint8_t)((channels[8] & 0x07FF) >> 8 | (channels[9] & 0x07FF) << 3); packet[13] = (uint8_t)((channels[7] & 0x07FF) >> 3);
packet[16] = (uint8_t)((channels[9] & 0x07FF) >> 5 | (channels[10] & 0x07FF) << 6); packet[14] = (uint8_t)((channels[8] & 0x07FF));
packet[17] = (uint8_t)((channels[10] & 0x07FF) >> 2); 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[18] = (uint8_t)((channels[10] & 0x07FF) >> 10 | (channels[11] & 0x07FF) << 1);
packet[19] = (uint8_t)((channels[11] & 0x07FF) >> 7 | (channels[12] & 0x07FF) << 4); 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[20] = (uint8_t)((channels[12] & 0x07FF) >> 4 | (channels[13] & 0x07FF) << 7);
packet[21] = (uint8_t)((channels[13] & 0x07FF) >> 1); // Channel not used by ELRS packet[21] = (uint8_t)((channels[13] & 0x07FF) >> 1);
packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); // Channel not used by ELRS packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2);
packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); // Channel not used by ELRS packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5);
packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); // Channel not used by ELRS packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3);
packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1); packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1);
} }
/** /**
* @brief Prepare CRSF command packet * @brief Prepare CRSF command packet
* *
* @param packet_cmd * packet format:
* @param command * 0 - header
* @param value * 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 const int ELRS_ADDR_RADIO = 0xEA;
packet_cmd[1] = 6; // length const int ELRS_TYPE_SETTINGS_WRITE = 0x2D;
packet_cmd[2] = ELRS_TYPE_SETTINGS_WRITE;
packet_cmd[3] = ELRS_ADDRESS; packet_cmd[0] = ELRS_ADDRESS;
packet_cmd[4] = ELRS_ADDR_RADIO; packet_cmd[1] = 6;
packet_cmd[5] = command; packet_cmd[2] = ELRS_TYPE_SETTINGS_WRITE;
packet_cmd[6] = value; packet_cmd[3] = ELRS_ADDRESS;
packet_cmd[7] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); 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 value Value (0-2048) to be set
* @param channel * @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; 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) if (elrs_init_done == true)
{ {
prepare_crsf_data_packet(crsf_packet, elrs_channels); 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 else
{ {
@ -155,21 +181,21 @@ void erls_send_data()
if (elrs_init_counter < 500) if (elrs_init_counter < 500)
{ {
prepare_crsf_data_packet(crsf_packet, elrs_channels); 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++; 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) else if (elrs_init_counter < 505)
{ {
prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE); prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, _packet_rate);
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
elrs_init_counter++; elrs_init_counter++;
} }
// Send command to update TX power to 100mW // Send command to update TX power
else if (elrs_init_counter < 510) else if (elrs_init_counter < 510)
{ {
prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER); prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, _tx_power);
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); _serial.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
elrs_init_counter++; elrs_init_counter++;
} }
else else

119
firmware/src/ElrsTx.h Normal file
View File

@ -0,0 +1,119 @@
/*
* =======================================================================================================
* -------------------------------------------------------------------------------------------------------
* ---####################-----###########-------###########-----############--############-############--
* --######################---#############-----#############---- -- - ---
* --###### ##---##### ###-----### #####---------##-------#######------#-------------
* -- -------------- --- ----- --- ----- ---------##-------#------------#-------------
* --#####--------------------#####------####-####------#####---------##-------###########--############--
* -- -------------------- ------ ------ --------- ------- -- --
* --#####--------------------#####--------#####--------#####---------------------------------------------
* -- -------------------- -------- -------- ---------------------------------------------
* --######--------------##---#####---------------------#####---------------- ERLS -----------------------
* --##################### ---#####---------------------#####---------------------------------------------
* ---################### ----#####---------------------#####---------------------------------------------
* --- ----- --------------------- ---------------------------------------------
* -------------------------------------------------------------------------------------------------------
* =======================================================================================================
*
* Copyright 2023 Christoffer Martinsson <cm@cmtec.se>
*
* 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 <Arduino.h>
// 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

101
firmware/src/IndicatorLed.cpp Normal file
View File

@ -0,0 +1,101 @@
/*
* =======================================================================================================
* -------------------------------------------------------------------------------------------------------
* ---####################-----###########-------###########-----############--############-############--
* --######################---#############-----#############---- -- - ---
* --###### ##---##### ###-----### #####---------##-------#######------#-------------
* -- -------------- --- ----- --- ----- ---------##-------#------------#-------------
* --#####--------------------#####------####-####------#####---------##-------###########--############--
* -- -------------------- ------ ------ --------- ------- -- --
* --#####--------------------#####--------#####--------#####---------------------------------------------
* -- -------------------- -------- -------- ---------------------------------------------
* --######--------------##---#####---------------------#####---------------- IndicatorLed ---------------
* --##################### ---#####---------------------#####---------------------------------------------
* ---################### ----#####---------------------#####---------------------------------------------
* --- ----- --------------------- ---------------------------------------------
* -------------------------------------------------------------------------------------------------------
* =======================================================================================================
*
* Copyright 2023 Christoffer Martinsson <cm@cmtec.se>
*
* 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;
}
}

56
firmware/src/erls.h → firmware/src/IndicatorLed.h
View File

@ -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. * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
*/ */
#ifndef INDICATORLED_H
#define INDICATORLED_H
#include <Arduino.h> #include <Arduino.h>
// CRSF const int LED_OFF = 0;
#define CRSF_MAX_CHANNEL 16 const int LED_ON = 1;
#define CRSF_TYPE_CHANNELS 0x16 const int LED_BLINK = 2;
#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
// ELRS command class IndicatorLed
#define ELRS_ADDRESS 0xEE {
#define ELRS_BIND_COMMAND 0xFF public:
#define ELRS_WIFI_COMMAND 0xFE IndicatorLed(int pin);
#define ELRS_PKT_RATE_COMMAND 0x01 void begin();
#define ELRS_TLM_RATIO_COMMAND 0x02 void on();
#define ELRS_SWITCH_MODE_COMMAND 0x03 void off();
#define ELRS_MODEL_MATCH_COMMAND 0x04 void blink();
#define ELRS_POWER_COMMAND 0x06 void update();
#define ELRS_BLE_JOYSTIC_COMMAND 17
#define ELRS_TYPE_SETTINGS_WRITE 0x2D private:
#define ELRS_ADDR_RADIO 0xEA int _pin;
#define ELRS_SERIAL_PORT Serial1 int led_mode = LED_OFF;
#define ELRS_SERIAL_BAUDRATE 400000 int current_state = LED_OFF;
#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
#endif
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();

164
firmware/src/cmdr_joystick.cpp
View File

@ -32,38 +32,25 @@
#include <Arduino.h> #include <Arduino.h>
#include <EEPROM.h> #include <EEPROM.h>
#include "erls.h" #include "ElrsTx.h"
#include "IndicatorLed.h"
#include "ResponsiveAnalogRead.h" #include "ResponsiveAnalogRead.h"
#define LED_MODE_OFF 0 IndicatorLed status_led(13);
#define LED_MODE_ON 1 IndicatorLed button_led_1(22);
#define LED_MODE_BLINK 2 IndicatorLed button_led_2(23);
#define LED_ON HIGH
#define LED_OFF LOW
const int STATUS_LED_PIN = 13; const int TIME_US_1ms = 1000;
bool status_led_on = false; const int TIME_US_5ms = 5000;
int status_led_mode = LED_MODE_BLINK; const long TIME_US_200ms = 200000;
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
unsigned long current_timestamp_micros = 0; unsigned long current_timestamp_micros = 0;
unsigned long process_data_timestamp_micros = 0; unsigned long process_data_timestamp_micros = 0;
unsigned long indicator_timestamp_micros = 0; unsigned long indicator_timestamp_micros = 0;
unsigned long send_usb_timestamp_micros = 0; unsigned long send_usb_timestamp_micros = 0;
unsigned long send_elrs_timestamp_micros = 0; unsigned long send_elrs_timestamp_micros = 0;
#define BUTTON_PRESSED LOW const int BUTTON_PRESSED = LOW;
#define BUTTON_NOT_PRESSED HIGH const int BUTTON_NOT_PRESSED = HIGH;
const int BUTTON_FRONT_LEFT_UPPER_PIN = 4; const int BUTTON_FRONT_LEFT_UPPER_PIN = 4;
const int BUTTON_FRONT_LEFT_LOWER_PIN = 5; const int BUTTON_FRONT_LEFT_LOWER_PIN = 5;
const int BUTTON_FRONT_RIGHT_UPPER_PIN = 6; 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_LEFT_LOWER_PIN = 8;
const int BUTTON_TOP_RIGHT_LOWER_PIN = 11; const int BUTTON_TOP_RIGHT_LOWER_PIN = 11;
#define GIMBAL_MODE_FRSKY_M7 0 const int GIMBAL_MODE_FRSKY_M7 = 0;
#define GIMBAL_MODE_FRSKY_M10 1 const int GIMBAL_MODE_FRSKY_M10 = 1;
const int GIMBAL_MODE_PIN = 21; const int GIMBAL_MODE_PIN = 21;
int gimbal_mode = GIMBAL_MODE_FRSKY_M7; int gimbal_mode = GIMBAL_MODE_FRSKY_M7;
@ -82,15 +69,15 @@ bool toggle_button_arm = false;
bool toggle_button_mode = false; bool toggle_button_mode = false;
int toggle_button_arm_previous_value = HIGH; int toggle_button_arm_previous_value = HIGH;
#define AXIS_10BIT_MAX 1023 const int AXIS_10BIT_MAX = 1023;
#define AXIS_10BIT_MIN 0 const int AXIS_10BIT_MIN = 0;
#define AXIS_10BIT_CENTER 512 const int AXIS_10BIT_CENTER = 512;
#define AXIS_12BIT_MAX 4096 const int AXIS_12BIT_MAX = 4096;
#define AXIS_12BIT_MIN 0 const int AXIS_12BIT_MIN = 0;
#define AXIS_12BIT_CENTER 2048 const int AXIS_12BIT_CENTER = 2048;
#define DEADZONE_X 50 const int DEADZONE_X = 50;
#define DEADZONE_Y 50 const int DEADZONE_Y = 50;
#define JOYSTICK_HAT_CENTER -1 const int JOYSTICK_HAT_CENTER = -1;
enum EEPROM_ADR enum EEPROM_ADR
{ {
@ -148,10 +135,12 @@ ResponsiveAnalogRead analog_y1(A1, true);
ResponsiveAnalogRead analog_x2(A3, true); ResponsiveAnalogRead analog_x2(A3, true);
ResponsiveAnalogRead analog_y2(A2, true); ResponsiveAnalogRead analog_y2(A2, true);
#define CALIBRATION_OFF 0 ElrsTx elrs(Serial1);
#define CALIBRATION_INIT 1
#define CALIBRATION_CENTER 2 const int CALIBRATION_OFF = 0;
#define CALIBRATION_MINMAX 3 const int CALIBRATION_INIT = 1;
const int CALIBRATION_CENTER = 2;
const int CALIBRATION_MINMAX = 3;
int joystick_calibration_mode = CALIBRATION_OFF; 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) void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2)
{ {
joystick_x1_12bit = AXIS_12BIT_CENTER; joystick_x1_12bit = AXIS_12BIT_CENTER;
joystick_y1_12bit = AXIS_12BIT_CENTER; joystick_y1_12bit = AXIS_12BIT_CENTER;
joystick_x2_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 * @brief Save joystick calibration values to EEPROM
*/ */
void send_erls_data() void send_elrs_data()
{ {
// Set ELRS analog channels // 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); elrs.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); elrs.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); elrs.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_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3);
// Set ELRS digital channels // Set ELRS digital channels
for (int i = 4; i < CRSF_MAX_CHANNEL; i++) 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_arm) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 4);
if (toggle_button_mode) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 5); if (toggle_button_mode) elrs.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_UPPER_PIN) == BUTTON_PRESSED) elrs.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_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.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 (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8);
// Send ELRS data // 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 &current_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() void setup()
{ {
/* Init HW */ /* Init HW */
pinMode(STATUS_LED_PIN, OUTPUT); status_led.begin();
digitalWrite(STATUS_LED_PIN, LED_OFF); status_led.blink();
button_led_1.begin();
pinMode(BUTTON_LED_1_PIN, OUTPUT); button_led_2.begin();
digitalWrite(BUTTON_LED_1_PIN, LED_OFF);
pinMode(BUTTON_LED_2_PIN, OUTPUT);
digitalWrite(BUTTON_LED_2_PIN, LED_OFF);
pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP); pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP);
pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP); pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP);
@ -579,7 +534,8 @@ void setup()
// Check if bootloader mode is enabled // Check if bootloader mode is enabled
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) 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); delay(200);
_reboot_Teensyduino_(); _reboot_Teensyduino_();
} }
@ -591,7 +547,7 @@ void setup()
} }
// Init ELRS // Init ELRS
ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE); elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW);
} }
void loop() void loop()
@ -620,41 +576,41 @@ void loop()
/* Update/Send ERLS data with about 1,6ms interval */ /* Update/Send ERLS data with about 1,6ms interval */
if (current_timestamp_micros >= send_elrs_timestamp_micros) 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; send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
} }
/* Update indicator with 200ms interval */ /* Update indicator with 200ms interval */
if (current_timestamp_micros >= indicator_timestamp_micros) if (current_timestamp_micros >= indicator_timestamp_micros)
{ {
button_led_1_mode = LED_MODE_OFF; button_led_1.off();
button_led_2_mode = LED_MODE_OFF; button_led_2.off();
if (joystick_calibration_mode == CALIBRATION_INIT) if (joystick_calibration_mode == CALIBRATION_INIT)
{ {
button_led_1_mode = LED_MODE_BLINK; button_led_1.blink();
button_led_2_mode = LED_MODE_BLINK; button_led_2.blink();
} }
else if (joystick_calibration_mode == CALIBRATION_CENTER) else if (joystick_calibration_mode == CALIBRATION_CENTER)
{ {
button_led_1_mode = LED_MODE_BLINK; button_led_1.blink();
button_led_2_mode = LED_MODE_OFF; button_led_2.off();
} }
else if (joystick_calibration_mode == CALIBRATION_MINMAX) else if (joystick_calibration_mode == CALIBRATION_MINMAX)
{ {
button_led_1_mode = LED_MODE_OFF; button_led_1.off();
button_led_2_mode = LED_MODE_BLINK; button_led_2.blink();
} }
else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) || 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)) (joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER))
{ {
button_led_1_mode = LED_MODE_ON; button_led_1.on();
button_led_2_mode = LED_MODE_ON; button_led_2.on();
} }
update_indicator(button_led_1_mode, button_led_1_on, BUTTON_LED_1_PIN); status_led.update();
update_indicator(button_led_2_mode, button_led_2_on, BUTTON_LED_2_PIN); button_led_1.update();
update_indicator(status_led_mode, status_led_on, STATUS_LED_PIN); button_led_2.update();
indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms; indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms;
} }