Code cleanup
This commit is contained in:
parent
86c205dcca
commit
dceae922de
@ -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++;
|
||||
}
|
||||
}
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
/*
|
||||
/*
|
||||
* =======================================================================================================
|
||||
* -------------------------------------------------------------------------------------------------------
|
||||
* ---####################-----###########-------###########-----############--############-############--
|
||||
@ -32,18 +32,18 @@
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// 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
|
||||
#endif
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
#endif
|
||||
|
||||
@ -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 <Arduino.h>
|
||||
#include <EEPROM.h>
|
||||
#include <ResponsiveAnalogRead.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user