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

Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-05-29 23:45:09 +02:00
parent b4ea94ab94
commit 785cf274ba
5 changed files with 583 additions and 503 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
firmware/src/ElrsTx.cpp
View File

@ -34,12 +34,15 @@
* *
* @param serial The serial port to use * @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) { void ElrsTx::begin(int packet_rate, int tx_power)
{
const int ELRS_SERIAL_BAUDRATE = 400000; const int ELRS_SERIAL_BAUDRATE = 400000;
_serial.begin(ELRS_SERIAL_BAUDRATE); _serial.begin(ELRS_SERIAL_BAUDRATE);
@ -55,9 +58,11 @@ void ElrsTx::begin(int packet_rate, int tx_power) {
* @param len The length of the packet * @param len The length of the packet
* @return The CRC8 of the packet * @return The CRC8 of the packet
*/ */
uint8_t ElrsTx::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;
@ -76,7 +81,8 @@ uint8_t ElrsTx::calculate_crsf_crc8(const uint8_t *ptr, uint8_t len) {
* @param packet Resulting packet * @param packet Resulting packet
* @param channels Channel data * @param channels Channel data
*/ */
void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) { void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[])
{
const int CRSF_TYPE_CHANNELS = 0x16; const int CRSF_TYPE_CHANNELS = 0x16;
packet[0] = ELRS_ADDRESS; packet[0] = ELRS_ADDRESS;
@ -132,7 +138,8 @@ void ElrsTx::prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) {
* @param command Command to be sent * @param command Command to be sent
* @param value Value 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) { 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_ADDR_RADIO = 0xEA;
const int ELRS_TYPE_SETTINGS_WRITE = 0x2D; const int ELRS_TYPE_SETTINGS_WRITE = 0x2D;
@ -152,37 +159,49 @@ void ElrsTx::prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint
* @param value Value (0-2048) to be set * @param value Value (0-2048) to be set
* @param channel Channel (0-11) 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 * @brief Send data to ERLS TX. This function should be called within the main loop with a 1.6ms interval
*/ */
void ElrsTx::send_data() { void ElrsTx::send_data()
{
const int ELRS_PKT_RATE_COMMAND = 0x01; const int ELRS_PKT_RATE_COMMAND = 0x01;
const int ELRS_POWER_COMMAND = 0x06; 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);
_serial.write(crsf_packet, CRSF_PACKET_SIZE); _serial.write(crsf_packet, CRSF_PACKET_SIZE);
} else { }
else
{
// Setting up initial communication link between ERLS TX and Teensy (give ERLS TX time to auto detect Teensy) // Setting up initial communication link between ERLS TX and Teensy (give ERLS TX time to auto detect Teensy)
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);
_serial.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 // 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, _packet_rate); prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, _packet_rate);
_serial.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 // 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, _tx_power); prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, _tx_power);
_serial.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
{
elrs_init_done = true; elrs_init_done = true;
} }
} }

3
firmware/src/ElrsTx.h
View File

@ -70,7 +70,8 @@ const int ELRS_TX_POWER_250mW = 3;
const int ELRS_TX_POWER_500mW = 4; const int ELRS_TX_POWER_500mW = 4;
const int ELRS_TX_POWER_1000mW = 5; const int ELRS_TX_POWER_1000mW = 5;
class ElrsTx { class ElrsTx
{
public: public:
ElrsTx(HardwareSerial &serial); ElrsTx(HardwareSerial &serial);

3
firmware/src/IndicatorLed.h
View File

@ -36,7 +36,8 @@ const int LED_OFF = 0;
const int LED_ON = 1; const int LED_ON = 1;
const int LED_BLINK = 2; const int LED_BLINK = 2;
class IndicatorLed { class IndicatorLed
{
public: public:
IndicatorLed(int pin); IndicatorLed(int pin);
void begin(); void begin();

155
firmware/src/cmdr_joystick.cpp
View File

@ -78,7 +78,8 @@ const int DEADZONE_X = 50;
const int DEADZONE_Y = 50; const int DEADZONE_Y = 50;
const int JOYSTICK_HAT_CENTER = -1; const int JOYSTICK_HAT_CENTER = -1;
enum EEPROM_ADR { enum EEPROM_ADR
{
MAX_X1_ADR_HIGH, MAX_X1_ADR_HIGH,
MAX_X1_ADR_LOW, MAX_X1_ADR_LOW,
MIN_X1_ADR_HIGH, MIN_X1_ADR_HIGH,
@ -144,7 +145,8 @@ int joystick_calibration_mode = CALIBRATION_OFF;
/** /**
* @brief Save calibration data to EEPROM * @brief Save calibration data to EEPROM
*/ */
void save_to_eeprom() { void save_to_eeprom()
{
EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max); EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max);
EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8); 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_LOW, joystick_x1_12bit_min);
@ -177,7 +179,8 @@ void save_to_eeprom() {
/** /**
* @brief Load calibration data from EEPROM * @brief Load calibration data from EEPROM
*/ */
void load_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_HIGH) << 8);
joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW); 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_HIGH) << 8);
@ -215,21 +218,25 @@ void load_from_eeprom() {
* @param analog_x2 * @param analog_x2
* @param analog_y2 * @param analog_y2
*/ */
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;
joystick_y2_12bit = AXIS_12BIT_CENTER; joystick_y2_12bit = AXIS_12BIT_CENTER;
// Check for calibration mode // Check for calibration mode
if (joystick_calibration_mode == CALIBRATION_INIT) { if (joystick_calibration_mode == CALIBRATION_INIT)
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED) { {
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED)
{
joystick_calibration_mode = CALIBRATION_CENTER; joystick_calibration_mode = CALIBRATION_CENTER;
} }
} }
// Calibrate joystick center values // Calibrate joystick center values
else if (joystick_calibration_mode == CALIBRATION_CENTER) { else if (joystick_calibration_mode == CALIBRATION_CENTER)
{
joystick_x1_12bit_center = analog_x1; joystick_x1_12bit_center = analog_x1;
joystick_y1_12bit_center = analog_y1; joystick_y1_12bit_center = analog_y1;
joystick_x1_12bit_max = joystick_x1_12bit_center; joystick_x1_12bit_max = joystick_x1_12bit_center;
@ -244,13 +251,15 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2)
joystick_y2_12bit_max = joystick_y2_12bit_center; joystick_y2_12bit_max = joystick_y2_12bit_center;
joystick_y2_12bit_min = joystick_y2_12bit_center; joystick_y2_12bit_min = joystick_y2_12bit_center;
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED)
{
joystick_calibration_mode = CALIBRATION_MINMAX; joystick_calibration_mode = CALIBRATION_MINMAX;
} }
} }
// Calibrate joystick min/max values // Calibrate joystick min/max values
else if (joystick_calibration_mode == CALIBRATION_MINMAX) { 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_max) joystick_x1_12bit_max = analog_x1;
if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = 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_max) joystick_y1_12bit_max = analog_y1;
@ -260,7 +269,8 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2)
if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2; 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 (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2;
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED)
{
joystick_calibration_mode = CALIBRATION_OFF; joystick_calibration_mode = CALIBRATION_OFF;
save_to_eeprom(); save_to_eeprom();
} }
@ -270,7 +280,8 @@ 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_elrs_data() { void send_elrs_data()
{
// Set ELRS analog channels // 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_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_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1);
@ -278,7 +289,8 @@ void send_elrs_data() {
elrs.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++)
{
elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i); elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i);
} }
@ -295,7 +307,8 @@ void send_elrs_data() {
/** /**
* Send USB data to PC * Send USB data to PC
*/ */
void send_usb_data() { void send_usb_data()
{
// Set USB analog channels // 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_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_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
@ -305,17 +318,22 @@ void send_usb_data() {
Joystick.Zrotate(joystick_x1_10bit); Joystick.Zrotate(joystick_x1_10bit);
Joystick.Z(joystick_y1_10bit); Joystick.Z(joystick_y1_10bit);
if (fn_mode == 2) { if (fn_mode == 2)
{
Joystick.X(AXIS_10BIT_CENTER); Joystick.X(AXIS_10BIT_CENTER);
Joystick.Y(AXIS_10BIT_CENTER); Joystick.Y(AXIS_10BIT_CENTER);
Joystick.sliderRight(joystick_x2_10bit); Joystick.sliderRight(joystick_x2_10bit);
Joystick.sliderLeft(joystick_y2_10bit); Joystick.sliderLeft(joystick_y2_10bit);
} else if (fn_mode == 1) { }
else if (fn_mode == 1)
{
Joystick.X(AXIS_10BIT_CENTER); Joystick.X(AXIS_10BIT_CENTER);
Joystick.Y(joystick_y2_10bit); Joystick.Y(joystick_y2_10bit);
Joystick.sliderRight(joystick_x2_10bit); Joystick.sliderRight(joystick_x2_10bit);
Joystick.sliderLeft(AXIS_10BIT_CENTER); Joystick.sliderLeft(AXIS_10BIT_CENTER);
} else { }
else
{
Joystick.X(joystick_x2_10bit); Joystick.X(joystick_x2_10bit);
Joystick.Y(joystick_y2_10bit); Joystick.Y(joystick_y2_10bit);
Joystick.sliderRight(AXIS_10BIT_CENTER); Joystick.sliderRight(AXIS_10BIT_CENTER);
@ -323,27 +341,33 @@ void send_usb_data() {
} }
// Set USB digital channels // Set USB digital channels
for (int i = 1; i < 32; i++) { for (int i = 1; i < 32; i++)
{
Joystick.button(i, 0); Joystick.button(i, 0);
} }
Joystick.hat(JOYSTICK_HAT_CENTER); Joystick.hat(JOYSTICK_HAT_CENTER);
if (fn_mode == 2) { if (fn_mode == 2)
{
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1); 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_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_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_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_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1); if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1);
} else if (fn_mode == 1) { }
else if (fn_mode == 1)
{
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 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_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_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_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_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
} else { }
else
{
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1); 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_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_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1);
@ -366,16 +390,21 @@ void send_usb_data() {
* @param deadband_value * @param deadband_value
* @param expo_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 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 calibrated_value = AXIS_12BIT_CENTER;
if (gimbal_value > (center_value + deadband_value)) { 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); 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)) { }
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); 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) { if (expo_value != 0)
{
float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX); float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX);
/* Calculate expo using 9th order polynomial function with 0.5 as center point */ /* 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; float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float;
@ -388,25 +417,30 @@ int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int
/** /**
* @brief Process input data from gimbal and buttons * @brief Process input data from gimbal and buttons
*/ */
void process_input_data() { void process_input_data()
{
int analog_x1_gimbal_value = 0; int analog_x1_gimbal_value = 0;
int analog_y1_gimbal_value = 0; int analog_y1_gimbal_value = 0;
int analog_x2_gimbal_value = 0; int analog_x2_gimbal_value = 0;
int analog_y2_gimbal_value = 0; int analog_y2_gimbal_value = 0;
if (gimbal_mode == GIMBAL_MODE_FRSKY_M10) { 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_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_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_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); 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) { }
else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7)
{
analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX); 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_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_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); analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
} }
if (joystick_calibration_mode != CALIBRATION_OFF) { if (joystick_calibration_mode != CALIBRATION_OFF)
{
calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value); calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value);
return; return;
} }
@ -418,32 +452,42 @@ void process_input_data() {
// Check fn mode // Check fn mode
fn_mode = 0; fn_mode = 0;
if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED)
{
fn_mode = 1; fn_mode = 1;
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED)
{
fn_mode = 2; fn_mode = 2;
} }
} }
// Check toggle mode buttons // Check toggle mode buttons
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED)
{
toggle_button_mode = true; toggle_button_mode = true;
} else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { }
else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED)
{
toggle_button_mode = false; toggle_button_mode = false;
} }
// Check toggle arm button // Check toggle arm button
if ((fn_mode == 1) && (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) != toggle_button_arm_previous_value)) { 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)) { {
if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == false))
{
toggle_button_arm = true; toggle_button_arm = true;
} else if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (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 = false;
} }
toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN); toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN);
} }
} }
void setup() { void setup()
{
/* Init HW */ /* Init HW */
status_led.begin(); status_led.begin();
status_led.blink(); status_led.blink();
@ -479,12 +523,14 @@ void setup() {
Joystick.useManualSend(true); Joystick.useManualSend(true);
// Check if calibration mode is enabled // Check if calibration mode is enabled
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) { if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED)
{
joystick_calibration_mode = CALIBRATION_INIT; joystick_calibration_mode = CALIBRATION_INIT;
} }
// 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)
{
button_led_2.on(); button_led_2.on();
button_led_2.update(); button_led_2.update();
delay(200); delay(200);
@ -492,7 +538,8 @@ void setup() {
} }
// Check what gimbal mode is selected // Check what gimbal mode is selected
if (digitalRead(GIMBAL_MODE_PIN) == LOW) { if (digitalRead(GIMBAL_MODE_PIN) == LOW)
{
gimbal_mode = GIMBAL_MODE_FRSKY_M10; gimbal_mode = GIMBAL_MODE_FRSKY_M10;
} }
@ -500,7 +547,8 @@ void setup() {
elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW); elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW);
} }
void loop() { void loop()
{
current_timestamp_micros = micros(); current_timestamp_micros = micros();
analog_x1.update(); analog_x1.update();
@ -509,38 +557,49 @@ void loop() {
analog_y2.update(); analog_y2.update();
/* Process data with 1ms interval*/ /* Process data with 1ms interval*/
if (current_timestamp_micros >= process_data_timestamp_micros) { if (current_timestamp_micros >= process_data_timestamp_micros)
{
process_input_data(); process_input_data();
process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms; process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms;
} }
/* Update/Send USB data with 5ms interval*/ /* Update/Send USB data with 5ms interval*/
if (current_timestamp_micros >= send_usb_timestamp_micros) { if (current_timestamp_micros >= send_usb_timestamp_micros)
{
send_usb_data(); send_usb_data();
send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms; send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms;
} }
/* 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_elrs_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.off(); button_led_1.off();
button_led_2.off(); button_led_2.off();
if (joystick_calibration_mode == CALIBRATION_INIT) { if (joystick_calibration_mode == CALIBRATION_INIT)
{
button_led_1.blink(); button_led_1.blink();
button_led_2.blink(); button_led_2.blink();
} else if (joystick_calibration_mode == CALIBRATION_CENTER) { }
else if (joystick_calibration_mode == CALIBRATION_CENTER)
{
button_led_1.blink(); button_led_1.blink();
button_led_2.off(); button_led_2.off();
} else if (joystick_calibration_mode == CALIBRATION_MINMAX) { }
else if (joystick_calibration_mode == CALIBRATION_MINMAX)
{
button_led_1.off(); button_led_1.off();
button_led_2.blink(); 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)) { }
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_1.on();
button_led_2.on(); button_led_2.on();
} }