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

Changed to new clang format

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-06-06 17:27:44 +02:00
parent 985b0d43f3
commit 727d76a25c
2 changed files with 49 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
firmware/lib/teensy-libc

@ -1 +1 @@
Subproject commit 6a2b179d8f624d69b41a5d30df7b46080a1deec6
Subproject commit 88cb7c10ca12ea33fb96aa10ae97a14d087da852

154
firmware/src/cmdr_joystick.cpp
View File

@ -55,8 +55,7 @@ const int DEADZONE_X = 50;
const int DEADZONE_Y = 50;
const int JOYSTICK_HAT_CENTER = -1;
enum EEPROM_ADR
{
enum EEPROM_ADR {
MAX_X1_ADR_HIGH,
MAX_X1_ADR_LOW,
MIN_X1_ADR_HIGH,
@ -122,8 +121,7 @@ int joystick_calibration_mode = CALIBRATION_OFF;
/**
* @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_HIGH, joystick_x1_12bit_max >> 8);
EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min);
@ -156,8 +154,7 @@ void save_to_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_LOW);
joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8);
@ -195,22 +192,19 @@ 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)
{
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 && digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED)
{
if (joystick_calibration_mode == CALIBRATION_INIT && 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)
{
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;
@ -225,15 +219,13 @@ 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_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;
}
}
// 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_min) joystick_x1_12bit_min = analog_x1;
if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1;
@ -243,8 +235,7 @@ 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_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;
save_to_eeprom();
}
@ -254,8 +245,7 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2)
/**
* @brief Save joystick calibration values to EEPROM
*/
void send_elrs_data()
{
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);
@ -263,8 +253,7 @@ 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);
// 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);
}
@ -281,8 +270,7 @@ void send_elrs_data()
/**
* Send USB data to PC
*/
void send_usb_data()
{
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);
@ -292,22 +280,17 @@ void send_usb_data()
Joystick.Zrotate(joystick_x1_10bit);
Joystick.Z(joystick_y1_10bit);
if (fn_mode == 2)
{
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)
{
} 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
{
} else {
Joystick.X(joystick_x2_10bit);
Joystick.Y(joystick_y2_10bit);
Joystick.sliderRight(AXIS_10BIT_CENTER);
@ -315,33 +298,27 @@ void send_usb_data()
}
// Set USB digital channels
for (int i = 1; i < 32; i++)
{
for (int i = 1; i < 32; i++) {
Joystick.button(i, 0);
}
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_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)
{
} 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
{
} 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);
@ -364,23 +341,18 @@ 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 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))
{
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))
{
} 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)
{
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;
@ -393,30 +365,25 @@ int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int
/**
* @brief Process input data from gimbal and buttons
*/
void process_input_data()
{
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)
{
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)
{
} 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)
{
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;
}
@ -432,42 +399,32 @@ void process_input_data()
// Check fn mode
fn_mode = 0;
if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED)
{
if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
fn_mode = 1;
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED)
{
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)
{
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) {
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;
}
// 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))
{
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))
{
} 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()
{
void setup() {
/* Init HW */
status_led.begin();
status_led.blink();
@ -503,14 +460,12 @@ void setup()
Joystick.useManualSend(true);
// 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;
}
// 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.update();
delay(200);
@ -518,8 +473,7 @@ void setup()
}
// Check what gimbal mode is selected
if (digitalRead(GIMBAL_MODE_PIN) == LOW)
{
if (digitalRead(GIMBAL_MODE_PIN) == LOW) {
gimbal_mode = GIMBAL_MODE_FRSKY_M10;
}
@ -527,8 +481,7 @@ void setup()
elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW);
}
void loop()
{
void loop() {
current_timestamp_micros = micros();
analog_x1.update();
@ -537,50 +490,39 @@ void loop()
analog_y2.update();
/* 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_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms;
}
/* 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_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)
{
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)
{
if (current_timestamp_micros >= indicator_timestamp_micros) {
button_led_1.off();
button_led_2.off();
if (joystick_calibration_mode == CALIBRATION_INIT)
{
if (joystick_calibration_mode == CALIBRATION_INIT) {
button_led_1.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_2.off();
}
else if (joystick_calibration_mode == CALIBRATION_MINMAX)
{
} 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))
{
} 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();
}