Code cleanup. Added support for two different gimbal (M10 and M7)
This commit is contained in:
parent
218e3a9048
commit
0ba7584d56
@ -138,39 +138,40 @@ int16_t elrs_channels[CRSF_MAX_CHANNEL];
|
||||
bool elrs_init_done = false;
|
||||
int elrs_init_counter = 0;
|
||||
|
||||
const int STATUS_LED = 13;
|
||||
const int STATUS_LED_PIN = 13;
|
||||
bool status_led_on = false;
|
||||
int status_led_mode = 2;
|
||||
|
||||
const int BUTTON_LED_1 = 22;
|
||||
const int BUTTON_LED_1_PIN = 22;
|
||||
bool button_led_1_on = false;
|
||||
int button_led_1_mode = 0;
|
||||
|
||||
const int BUTTON_LED_2 = 23;
|
||||
const int BUTTON_LED_2_PIN = 23;
|
||||
bool button_led_2_on = false;
|
||||
int button_led_2_mode = 0;
|
||||
|
||||
unsigned long current_timestamp = 0;
|
||||
unsigned long led_timestamp = 0;
|
||||
unsigned long process_data_timestamp = 0;
|
||||
unsigned long indicator_timestamp = 0;
|
||||
unsigned long send_usb_timestamp = 0;
|
||||
|
||||
unsigned long current_timestamp_micros = 0;
|
||||
unsigned long send_elrs_timestamp_micros = 0;
|
||||
|
||||
const int BUTTON_FLU = 4; // Front left upper button
|
||||
const int BUTTON_FLD = 5; // Front left lower button
|
||||
const int BUTTON_FRU = 6; // Front right upper button
|
||||
const int BUTTON_FRD = 7; // Front right lower button
|
||||
const int BUTTON_TLU = 9; // top upper left button
|
||||
const int BUTTON_TRU = 10; // Top upper right button
|
||||
const int BUTTON_TLD = 8; // Top left lower button
|
||||
const int BUTTON_TRD = 11; // Top right lower button
|
||||
const int BUTTON_FLU_PIN = 4; // Front left upper button
|
||||
const int BUTTON_FLD_PIN = 5; // Front left lower button
|
||||
const int BUTTON_FRU_PIN = 6; // Front right upper button
|
||||
const int BUTTON_FRD_PIN = 7; // Front right lower button
|
||||
const int BUTTON_TLU_PIN = 9; // top upper left button
|
||||
const int BUTTON_TRU_PIN = 10; // Top upper right button
|
||||
const int BUTTON_TLD_PIN = 8; // Top left lower button
|
||||
const int BUTTON_TRD_PIN = 11; // Top right lower button
|
||||
|
||||
const int GIMBAL_MODE_PIN = 21; // Open = M7, Closed (GND) = M10
|
||||
|
||||
bool toggle_button_arm = false;
|
||||
bool toggle_button_mode = false;
|
||||
int button_arm_previous_value = HIGH;
|
||||
int button_11_previous_value = HIGH;
|
||||
int toggle_button_arm_previous_value = HIGH;
|
||||
|
||||
#define HID_AXIS_MAX 1023
|
||||
#define HID_AXIS_MIN 0
|
||||
@ -213,7 +214,6 @@ enum EEPROM_ADR
|
||||
int joystick_counter = 0;
|
||||
int joystick_x1 = 0;
|
||||
int joystick_x1_hid = 0;
|
||||
int joystick_x1_raw = 0;
|
||||
int joystick_x1_max = 4096;
|
||||
int joystick_x1_min = 0;
|
||||
int joystick_x1_center = joystick_x1_max/2;
|
||||
@ -221,19 +221,16 @@ int joystick_y1 = 0;
|
||||
int joystick_y1_hid = 0;
|
||||
int joystick_y1_hid_rev = 0;
|
||||
int joystick_y1_hid_rev_mode = 0;
|
||||
int joystick_y1_raw = 0;
|
||||
int joystick_y1_max = 4096;
|
||||
int joystick_y1_min = 0;
|
||||
int joystick_y1_center = joystick_y1_max/2;
|
||||
int joystick_x2 = 0;
|
||||
int joystick_x2_hid = 0;
|
||||
int joystick_x2_raw = 0;
|
||||
int joystick_x2_max = 4096;
|
||||
int joystick_x2_min = 0;
|
||||
int joystick_x2_center = joystick_x2_max/2;
|
||||
int joystick_y2 = 0;
|
||||
int joystick_y2_hid = 0;
|
||||
int joystick_y2_raw = 0;
|
||||
int joystick_y2_max = 4096;
|
||||
int joystick_y2_min = 0;
|
||||
int joystick_y2_center = joystick_y2_max/2;
|
||||
@ -241,11 +238,12 @@ int joystick_y2_center = joystick_y2_max/2;
|
||||
float exp_constant = 0.2;
|
||||
|
||||
int fn_mode = 0;
|
||||
int gimbal_mode = 0;
|
||||
|
||||
ResponsiveAnalogRead analog_x1(A0, true);
|
||||
ResponsiveAnalogRead analog_y1(A1, true);
|
||||
ResponsiveAnalogRead analog_x2(A2, true);
|
||||
ResponsiveAnalogRead analog_y2(A3, true);
|
||||
ResponsiveAnalogRead analog_x2(A3, true);
|
||||
ResponsiveAnalogRead analog_y2(A2, true);
|
||||
|
||||
#define CALIBRATION_OFF 0
|
||||
#define CALIBRATION_INIT 1
|
||||
@ -315,211 +313,8 @@ void load_from_eeprom(){
|
||||
joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW);
|
||||
}
|
||||
|
||||
void read_io_data(){
|
||||
|
||||
if(joystick_counter == 0){
|
||||
joystick_x1_raw = analog_x1.getValue();
|
||||
joystick_counter++;
|
||||
}
|
||||
else if(joystick_counter == 1){
|
||||
joystick_y1_raw = analog_y1.getValue();
|
||||
joystick_counter++;
|
||||
}
|
||||
else if(joystick_counter == 2){
|
||||
joystick_x2_raw = analog_x2.getValue();
|
||||
joystick_counter++;
|
||||
}
|
||||
else if(joystick_counter == 3){
|
||||
joystick_y2_raw = analog_y2.getValue();
|
||||
joystick_counter = 0;
|
||||
|
||||
if (joystick_calibration_mode == CALIBRATION_OFF){
|
||||
|
||||
// Calculate X1 joystick values
|
||||
if(joystick_x1_raw > (joystick_x1_center + DEADZONE_X)){
|
||||
joystick_x1 = constrain(map(joystick_x1_raw, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(joystick_x1_raw < (joystick_x1_center - DEADZONE_X)){
|
||||
joystick_x1 = constrain(map(joystick_x1_raw, joystick_x1_min, (joystick_x1_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_x1 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate Y1 joystick values
|
||||
if(joystick_y1_raw > (joystick_y1_min + DEADZONE_Y)){
|
||||
joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_min + DEADZONE_Y), joystick_y1_max, AXIS_MIN, AXIS_MAX), AXIS_MIN, AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y1 = AXIS_MIN;
|
||||
}
|
||||
|
||||
// Calculate X2 joystick values
|
||||
if(joystick_x2_raw > (joystick_x2_center + DEADZONE_X)){
|
||||
joystick_x2 = constrain(map(joystick_x2_raw, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(joystick_x2_raw < (joystick_x2_center - DEADZONE_X)){
|
||||
joystick_x2 = constrain(map(joystick_x2_raw, joystick_x2_min, (joystick_x2_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_x2 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate Y2 joystick values
|
||||
if(joystick_y2_raw > (joystick_y2_center + DEADZONE_Y)){
|
||||
joystick_y2 = constrain(map(joystick_y2_raw, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(joystick_y2_raw < (joystick_y2_center - DEADZONE_Y)){
|
||||
joystick_y2 = constrain(map(joystick_y2_raw, joystick_y2_min, (joystick_y2_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_y2 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new X1 values after applying exp curve
|
||||
if (joystick_x1 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x1 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x1_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x1_hid = constrain(joystick_x1_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y1 values
|
||||
joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
|
||||
// Calculate new X2 values after applying exp curve
|
||||
if (joystick_x2 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x2 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x2_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x2_hid = constrain(joystick_x2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y2 values after applying exp curve
|
||||
if (joystick_y2 != AXIS_CENTER){
|
||||
float joystick_y_float = joystick_y2 / float(AXIS_MAX);
|
||||
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
|
||||
joystick_y2_hid = int(joystick_y_exp * float(HID_AXIS_MAX));
|
||||
joystick_y2_hid = constrain(joystick_y2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Check fn mode
|
||||
fn_mode = 0;
|
||||
if (digitalRead(BUTTON_TLU) == LOW){
|
||||
fn_mode = 1;
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
fn_mode = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// Check toggle buttons
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
toggle_button_mode = true;
|
||||
}
|
||||
else if (digitalRead(BUTTON_FLD) == LOW){
|
||||
toggle_button_mode = false;
|
||||
}
|
||||
|
||||
if (fn_mode == 1){
|
||||
if (digitalRead(BUTTON_TRD) != button_arm_previous_value){
|
||||
button_arm_previous_value = digitalRead(BUTTON_TRD);
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
if (toggle_button_arm == false){
|
||||
toggle_button_arm = true;
|
||||
}
|
||||
else{
|
||||
toggle_button_arm = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
// Calibration mode.
|
||||
joystick_x1 = AXIS_CENTER;
|
||||
joystick_y1 = AXIS_CENTER;
|
||||
joystick_x2 = AXIS_CENTER;
|
||||
joystick_y2 = AXIS_CENTER;
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
joystick_y1_hid = HID_AXIS_CENTER;
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
|
||||
// Check for calibration mode
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
if (digitalRead(BUTTON_TLD) == HIGH){
|
||||
joystick_calibration_mode = CALIBRATION_CENTER;
|
||||
}
|
||||
}
|
||||
// Calibrate joystick center values
|
||||
else if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
joystick_x1_center = joystick_x1_raw;
|
||||
joystick_y1_center = joystick_y1_raw;
|
||||
joystick_x1_max = joystick_x1_center;
|
||||
joystick_x1_min = joystick_x1_center;
|
||||
joystick_y1_max = joystick_y1_center;
|
||||
joystick_y1_min = joystick_y1_center;
|
||||
|
||||
joystick_x2_center = joystick_x2_raw;
|
||||
joystick_y2_center = joystick_y2_raw;
|
||||
joystick_x2_max = joystick_x2_center;
|
||||
joystick_x2_min = joystick_x2_center;
|
||||
joystick_y2_max = joystick_y2_center;
|
||||
joystick_y2_min = joystick_y2_center;
|
||||
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_MINMAX;
|
||||
}
|
||||
}
|
||||
// Calibrate joystick min/max values
|
||||
else if (joystick_calibration_mode == CALIBRATION_MINMAX){
|
||||
if(joystick_x1_raw > joystick_x1_max){
|
||||
joystick_x1_max = joystick_x1_raw;
|
||||
}
|
||||
if(joystick_x1_raw < joystick_x1_min){
|
||||
joystick_x1_min = joystick_x1_raw;
|
||||
}
|
||||
if(joystick_y1_raw > joystick_y1_max){
|
||||
joystick_y1_max = joystick_y1_raw;
|
||||
}
|
||||
if(joystick_y1_raw < joystick_y1_min){
|
||||
joystick_y1_min = joystick_y1_raw;
|
||||
}
|
||||
|
||||
if(joystick_x2_raw > joystick_x2_max){
|
||||
joystick_x2_max = joystick_x2_raw;
|
||||
}
|
||||
if(joystick_x2_raw < joystick_x2_min){
|
||||
joystick_x2_min = joystick_x2_raw;
|
||||
}
|
||||
if(joystick_y2_raw > joystick_y2_max){
|
||||
joystick_y2_max = joystick_y2_raw;
|
||||
}
|
||||
if(joystick_y2_raw < joystick_y2_min){
|
||||
joystick_y2_min = joystick_y2_raw;
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_OFF;
|
||||
save_to_eeprom();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// CRSF CRC calculation via lookup table
|
||||
uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) {
|
||||
uint8_t 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++];
|
||||
@ -528,7 +323,7 @@ uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) {
|
||||
}
|
||||
|
||||
// Prepare CRSF data packet
|
||||
void crsf_prepare_data_packet(uint8_t packet[], int16_t channels[]) {
|
||||
void prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) {
|
||||
packet[0] = ELRS_ADDRESS; // Header
|
||||
packet[1] = 24; // length
|
||||
packet[2] = CRSF_TYPE_CHANNELS;
|
||||
@ -554,11 +349,11 @@ void crsf_prepare_data_packet(uint8_t packet[], int16_t channels[]) {
|
||||
packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); // Channel not used by ELRS
|
||||
packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); // Channel not used by ELRS
|
||||
packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); // Channel not used by ELRS
|
||||
packet[25] = crsf_crc8(&packet[2], packet[1] - 1);
|
||||
packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1);
|
||||
}
|
||||
|
||||
// Prepare CRSF setup packet
|
||||
void crsf_prepare_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) {
|
||||
void prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) {
|
||||
packet_cmd[0] = ELRS_ADDRESS; // Header
|
||||
packet_cmd[1] = 6; // length
|
||||
packet_cmd[2] = ELRS_TYPE_SETTINGS_WRITE;
|
||||
@ -566,35 +361,424 @@ void crsf_prepare_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t valu
|
||||
packet_cmd[4] = ELRS_ADDR_RADIO;
|
||||
packet_cmd[5] = command;
|
||||
packet_cmd[6] = value;
|
||||
packet_cmd[7] = crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1);
|
||||
packet_cmd[7] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1);
|
||||
}
|
||||
|
||||
void send_elrs_data(){
|
||||
|
||||
// Set ELRS analog channels
|
||||
elrs_channels[0] = map(joystick_x1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[1] = map(joystick_y1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[2] = map(joystick_x2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[3] = map(joystick_y2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
|
||||
// Set ELRS digital channels
|
||||
for (int i = 4; i < CRSF_MAX_CHANNEL; i++){
|
||||
elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN;
|
||||
}
|
||||
if (toggle_button_arm){
|
||||
elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (toggle_button_mode){
|
||||
elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU_PIN) == LOW){
|
||||
elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD_PIN) == LOW){
|
||||
elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU_PIN) == LOW){
|
||||
elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
|
||||
// Send ELRS data
|
||||
if (elrs_init_done == true){
|
||||
prepare_crsf_data_packet(crsf_packet, elrs_channels);
|
||||
ELRS_SERIAL_PORT.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);
|
||||
ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX packet rate to 500Hz
|
||||
else if (elrs_init_counter < 505){
|
||||
prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX power to 100mW
|
||||
else if (elrs_init_counter < 510){
|
||||
prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
else{
|
||||
elrs_init_done = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void send_usb_data(){
|
||||
|
||||
// Set joystick buttons to released
|
||||
for (int i = 1; i < 32; i++){
|
||||
Joystick.button(i, 0);
|
||||
}
|
||||
|
||||
// Set joystick hat to center position
|
||||
Joystick.hat(-1);
|
||||
|
||||
// Set joystick buttons to pressed
|
||||
if (fn_mode == 2){
|
||||
|
||||
if (digitalRead(BUTTON_FLD_PIN) == LOW){
|
||||
Joystick.button(12, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD_PIN) == LOW){
|
||||
Joystick.button(13, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD_PIN) == LOW){
|
||||
Joystick.button(14, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
Joystick.button(15, 1);
|
||||
}
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
|
||||
if (digitalRead(BUTTON_FLD_PIN) == LOW){
|
||||
Joystick.button(8, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD_PIN) == LOW){
|
||||
Joystick.button(9, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD_PIN) == LOW){
|
||||
Joystick.button(10, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
Joystick.button(11, 1);
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
if (digitalRead(BUTTON_FLD_PIN) == LOW){
|
||||
Joystick.button(2, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD_PIN) == LOW){
|
||||
Joystick.button(4, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU_PIN) == LOW){
|
||||
Joystick.button(5, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD_PIN) == LOW){
|
||||
Joystick.button(6, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
Joystick.button(7, 1);
|
||||
}
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_FLU_PIN) == LOW){
|
||||
Joystick.button(1, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU_PIN) == LOW){
|
||||
Joystick.button(3, 1);
|
||||
}
|
||||
|
||||
// Set axis values
|
||||
Joystick.Zrotate(joystick_x1_hid);
|
||||
Joystick.Z(joystick_y1_hid);
|
||||
|
||||
if (fn_mode == 2){
|
||||
Joystick.X(HID_AXIS_CENTER);
|
||||
Joystick.Y(HID_AXIS_CENTER);
|
||||
Joystick.sliderRight(joystick_x2_hid);
|
||||
Joystick.sliderLeft(joystick_y2_hid);
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
Joystick.X(joystick_x2_hid);
|
||||
Joystick.Y(HID_AXIS_CENTER);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
Joystick.sliderLeft(joystick_y2_hid);
|
||||
}
|
||||
else{
|
||||
Joystick.X(joystick_x2_hid);
|
||||
Joystick.Y(joystick_y2_hid);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
Joystick.sliderLeft(HID_AXIS_CENTER);
|
||||
}
|
||||
|
||||
// Send USB Joystick data
|
||||
Joystick.send_now();
|
||||
}
|
||||
|
||||
void process_input_data(){
|
||||
|
||||
int analog_x1_inv = 0;
|
||||
int analog_y1_inv = 0;
|
||||
int analog_x2_inv = 0;
|
||||
int analog_y2_inv = 0;
|
||||
|
||||
if (gimbal_mode == 1){
|
||||
// FrSky M10 Gimbal
|
||||
analog_x1_inv = constrain(AXIS_MAX - analog_x1.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_y1_inv = constrain(analog_y1.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_x2_inv = constrain(analog_x2.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_y2_inv = constrain(AXIS_MAX - analog_y2.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
// FrSky M7 Gimbal
|
||||
analog_x1_inv = constrain(analog_x1.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_y1_inv = constrain(AXIS_MAX - analog_y1.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_x2_inv = constrain(AXIS_MAX - analog_x2.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
analog_y2_inv = constrain(analog_y2.getValue(), AXIS_MIN, AXIS_MAX);
|
||||
}
|
||||
|
||||
if (joystick_calibration_mode == CALIBRATION_OFF){
|
||||
|
||||
// Calculate X1 joystick values
|
||||
if(analog_x1_inv > (joystick_x1_center + DEADZONE_X)){
|
||||
joystick_x1 = constrain(map(analog_x1_inv, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(analog_x1_inv < (joystick_x1_center - DEADZONE_X)){
|
||||
joystick_x1 = constrain(map(analog_x1_inv, joystick_x1_min, (joystick_x1_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_x1 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate Y1 joystick values
|
||||
if(analog_y1_inv > (joystick_y1_min + DEADZONE_Y)){
|
||||
joystick_y1 = constrain(map(analog_y1_inv, (joystick_y1_min + DEADZONE_Y), joystick_y1_max, AXIS_MIN, AXIS_MAX), AXIS_MIN, AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y1 = AXIS_MIN;
|
||||
}
|
||||
|
||||
// Calculate X2 joystick values
|
||||
if(analog_x2_inv > (joystick_x2_center + DEADZONE_X)){
|
||||
joystick_x2 = constrain(map(analog_x2_inv, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(analog_x2_inv < (joystick_x2_center - DEADZONE_X)){
|
||||
joystick_x2 = constrain(map(analog_x2_inv, joystick_x2_min, (joystick_x2_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_x2 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate Y2 joystick values
|
||||
if(analog_y2_inv > (joystick_y2_center + DEADZONE_Y)){
|
||||
joystick_y2 = constrain(map(analog_y2_inv, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
|
||||
}
|
||||
else if(analog_y2_inv < (joystick_y2_center - DEADZONE_Y)){
|
||||
joystick_y2 = constrain(map(analog_y2_inv, joystick_y2_min, (joystick_y2_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
joystick_y2 = AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new X1 values after applying exp curve
|
||||
if (joystick_x1 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x1 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x1_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x1_hid = constrain(joystick_x1_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y1 values
|
||||
joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
|
||||
// Calculate new X2 values after applying exp curve
|
||||
if (joystick_x2 != AXIS_CENTER){
|
||||
float joystick_x_float = joystick_x2 / float(AXIS_MAX);
|
||||
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
|
||||
joystick_x2_hid = int(joystick_x_exp * float(HID_AXIS_MAX));
|
||||
joystick_x2_hid = constrain(joystick_x2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Calculate new Y2 values after applying exp curve
|
||||
if (joystick_y2 != AXIS_CENTER){
|
||||
float joystick_y_float = joystick_y2 / float(AXIS_MAX);
|
||||
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
|
||||
joystick_y2_hid = int(joystick_y_exp * float(HID_AXIS_MAX));
|
||||
joystick_y2_hid = constrain(joystick_y2_hid, HID_AXIS_MIN, HID_AXIS_MAX);
|
||||
}
|
||||
else{
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
}
|
||||
|
||||
// Check fn mode
|
||||
fn_mode = 0;
|
||||
if (digitalRead(BUTTON_TLU_PIN) == LOW){
|
||||
fn_mode = 1;
|
||||
if (digitalRead(BUTTON_TRU_PIN) == LOW){
|
||||
fn_mode = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// Check toggle mode buttons
|
||||
if (digitalRead(BUTTON_FLU_PIN) == LOW){
|
||||
toggle_button_mode = true;
|
||||
}
|
||||
else if (digitalRead(BUTTON_FLD_PIN) == LOW){
|
||||
toggle_button_mode = false;
|
||||
}
|
||||
|
||||
// Check toggle arm button
|
||||
if (fn_mode == 1){
|
||||
if (digitalRead(BUTTON_TRD_PIN) != toggle_button_arm_previous_value){
|
||||
toggle_button_arm_previous_value = digitalRead(BUTTON_TRD_PIN);
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
if (toggle_button_arm == false){
|
||||
toggle_button_arm = true;
|
||||
}
|
||||
else{
|
||||
toggle_button_arm = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
// Calibration mode.
|
||||
joystick_x1 = AXIS_CENTER;
|
||||
joystick_y1 = AXIS_CENTER;
|
||||
joystick_x2 = AXIS_CENTER;
|
||||
joystick_y2 = AXIS_CENTER;
|
||||
joystick_x1_hid = HID_AXIS_CENTER;
|
||||
joystick_y1_hid = HID_AXIS_CENTER;
|
||||
joystick_x2_hid = HID_AXIS_CENTER;
|
||||
joystick_y2_hid = HID_AXIS_CENTER;
|
||||
|
||||
// Check for calibration mode
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
if (digitalRead(BUTTON_TLD_PIN) == HIGH){
|
||||
joystick_calibration_mode = CALIBRATION_CENTER;
|
||||
}
|
||||
}
|
||||
|
||||
// Calibrate joystick center values
|
||||
else if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
joystick_x1_center = analog_x1_inv;
|
||||
joystick_y1_center = analog_y1_inv;
|
||||
joystick_x1_max = joystick_x1_center;
|
||||
joystick_x1_min = joystick_x1_center;
|
||||
joystick_y1_max = joystick_y1_center;
|
||||
joystick_y1_min = joystick_y1_center;
|
||||
|
||||
joystick_x2_center = analog_x2_inv;
|
||||
joystick_y2_center = analog_y2_inv;
|
||||
joystick_x2_max = joystick_x2_center;
|
||||
joystick_x2_min = joystick_x2_center;
|
||||
joystick_y2_max = joystick_y2_center;
|
||||
joystick_y2_min = joystick_y2_center;
|
||||
|
||||
if (digitalRead(BUTTON_TLD_PIN) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_MINMAX;
|
||||
}
|
||||
}
|
||||
|
||||
// Calibrate joystick min/max values
|
||||
else if (joystick_calibration_mode == CALIBRATION_MINMAX){
|
||||
if(analog_x1_inv > joystick_x1_max){
|
||||
joystick_x1_max = analog_x1_inv;
|
||||
}
|
||||
if(analog_x1_inv < joystick_x1_min){
|
||||
joystick_x1_min = analog_x1_inv;
|
||||
}
|
||||
if(analog_y1_inv > joystick_y1_max){
|
||||
joystick_y1_max = analog_y1_inv;
|
||||
}
|
||||
if(analog_y1_inv < joystick_y1_min){
|
||||
joystick_y1_min = analog_y1_inv;
|
||||
}
|
||||
|
||||
if(analog_x2_inv > joystick_x2_max){
|
||||
joystick_x2_max = analog_x2_inv;
|
||||
}
|
||||
if(analog_x2_inv < joystick_x2_min){
|
||||
joystick_x2_min = analog_x2_inv;
|
||||
}
|
||||
if(analog_y2_inv > joystick_y2_max){
|
||||
joystick_y2_max = analog_y2_inv;
|
||||
}
|
||||
if(analog_y2_inv < joystick_y2_min){
|
||||
joystick_y2_min = analog_y2_inv;
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_OFF;
|
||||
save_to_eeprom();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Update indicator 0=Off, 1=On, 2=Blink */
|
||||
void update_indicator(int mode, bool ¤t_state, int pin){
|
||||
|
||||
if (mode == 2 && current_state == false)
|
||||
{
|
||||
digitalWrite(pin, HIGH);
|
||||
current_state = true;
|
||||
}
|
||||
else if (mode == 2 && current_state == true)
|
||||
{
|
||||
digitalWrite(pin, LOW);
|
||||
current_state = false;
|
||||
}
|
||||
else if (mode == 1)
|
||||
{
|
||||
digitalWrite(pin, HIGH);
|
||||
current_state = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(pin, LOW);
|
||||
current_state = false;
|
||||
}
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
/* Init HW */
|
||||
pinMode(STATUS_LED, OUTPUT);
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
pinMode(STATUS_LED_PIN, OUTPUT);
|
||||
digitalWrite(STATUS_LED_PIN, LOW);
|
||||
|
||||
pinMode(BUTTON_LED_1, OUTPUT);
|
||||
digitalWrite(BUTTON_LED_1, LOW);
|
||||
pinMode(BUTTON_LED_1_PIN, OUTPUT);
|
||||
digitalWrite(BUTTON_LED_1_PIN, LOW);
|
||||
|
||||
pinMode(BUTTON_LED_2, OUTPUT);
|
||||
digitalWrite(BUTTON_LED_2, LOW);
|
||||
pinMode(BUTTON_LED_2_PIN, OUTPUT);
|
||||
digitalWrite(BUTTON_LED_2_PIN, LOW);
|
||||
|
||||
pinMode(BUTTON_FLD, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FLU, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FRD, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FRU, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TLD, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TRD, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TLU, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TRU, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FLD_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FLU_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FRD_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_FRU_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TLD_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TRD_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TLU_PIN, INPUT_PULLUP);
|
||||
pinMode(BUTTON_TRU_PIN, INPUT_PULLUP);
|
||||
|
||||
pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP);
|
||||
|
||||
// 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);
|
||||
@ -607,17 +791,22 @@ void setup()
|
||||
Joystick.useManualSend(true);
|
||||
|
||||
// Check if calibration mode is enabled
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
if (digitalRead(BUTTON_TLD_PIN) == LOW){
|
||||
joystick_calibration_mode = CALIBRATION_INIT;
|
||||
}
|
||||
|
||||
// Check if bootloader mode is enabled
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
digitalWrite(BUTTON_LED_2, HIGH);
|
||||
if (digitalRead(BUTTON_TRD_PIN) == LOW){
|
||||
digitalWrite(BUTTON_LED_2_PIN, HIGH);
|
||||
delay(200);
|
||||
_reboot_Teensyduino_();
|
||||
}
|
||||
|
||||
// Check what gimbal mode is selected
|
||||
if (digitalRead(GIMBAL_MODE_PIN) == LOW){
|
||||
gimbal_mode = 1;
|
||||
}
|
||||
|
||||
// Init ELRS
|
||||
ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE);
|
||||
}
|
||||
@ -629,179 +818,43 @@ void loop()
|
||||
/* Update current time (us) */
|
||||
current_timestamp_micros = micros();
|
||||
|
||||
/* Read io value and process as fast as possible */
|
||||
/* Read io value as fast as possible */
|
||||
analog_x1.update();
|
||||
analog_y1.update();
|
||||
analog_x2.update();
|
||||
analog_y2.update();
|
||||
|
||||
read_io_data();
|
||||
/* Process data with 1ms interval*/
|
||||
if (current_timestamp >= process_data_timestamp)
|
||||
{
|
||||
process_input_data();
|
||||
process_data_timestamp = current_timestamp + 1;
|
||||
}
|
||||
|
||||
/* Update/Send USB data with 5ms interval*/
|
||||
if (current_timestamp >= send_usb_timestamp)
|
||||
{
|
||||
// Set joystick buttons to released
|
||||
for (int i = 1; i < 32; i++){
|
||||
Joystick.button(i, 0);
|
||||
}
|
||||
|
||||
// Set joystick hat to center position
|
||||
Joystick.hat(-1);
|
||||
|
||||
// Set joystick buttons to pressed
|
||||
if (fn_mode == 2){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(12, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(13, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(14, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(15, 1);
|
||||
}
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(8, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(9, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(10, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(11, 1);
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
if (digitalRead(BUTTON_FLD) == LOW){
|
||||
Joystick.button(2, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
Joystick.button(4, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
Joystick.button(5, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TLD) == LOW){
|
||||
Joystick.button(6, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_TRD) == LOW){
|
||||
Joystick.button(7, 1);
|
||||
}
|
||||
}
|
||||
|
||||
if (digitalRead(BUTTON_FLU) == LOW){
|
||||
Joystick.button(1, 1);
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
Joystick.button(3, 1);
|
||||
}
|
||||
|
||||
// Set axis values
|
||||
Joystick.Y(joystick_y1_hid);
|
||||
Joystick.X(joystick_x1_hid);
|
||||
|
||||
if (fn_mode == 2){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(HID_AXIS_CENTER);
|
||||
Joystick.sliderLeft(joystick_x2_hid);
|
||||
Joystick.sliderRight(joystick_y2_hid);
|
||||
}
|
||||
else if (fn_mode == 1){
|
||||
Joystick.Z(HID_AXIS_CENTER);
|
||||
Joystick.Zrotate(joystick_y2_hid);
|
||||
Joystick.sliderLeft(joystick_x2_hid);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
else{
|
||||
Joystick.Z(joystick_x2_hid);
|
||||
Joystick.Zrotate(joystick_y2_hid);
|
||||
Joystick.sliderLeft(HID_AXIS_CENTER);
|
||||
Joystick.sliderRight(HID_AXIS_CENTER);
|
||||
}
|
||||
|
||||
// Send USB Joystick data
|
||||
Joystick.send_now();
|
||||
send_usb_data();
|
||||
send_usb_timestamp = current_timestamp + 5;
|
||||
}
|
||||
|
||||
/* Send data every CRSF_TIME_BETWEEN_FRAMES_US */
|
||||
/* Update/Send ERLS data with 1,6ms interval */
|
||||
if (current_timestamp_micros >= send_elrs_timestamp_micros)
|
||||
{
|
||||
// Set ELRS analog channels
|
||||
elrs_channels[0] = map(joystick_x1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[1] = map(joystick_y1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[2] = map(joystick_x2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
elrs_channels[3] = map(joystick_y2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX);
|
||||
|
||||
// Set ELRS digital channels
|
||||
for (int i = 4; i < CRSF_MAX_CHANNEL; i++){
|
||||
elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN;
|
||||
}
|
||||
if (toggle_button_arm){
|
||||
elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (toggle_button_mode){
|
||||
elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRU) == LOW){
|
||||
elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_FRD) == LOW){
|
||||
elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
if (digitalRead(BUTTON_TRU) == LOW){
|
||||
elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX;
|
||||
}
|
||||
|
||||
// Send ELRS data
|
||||
if (elrs_init_done == true){
|
||||
crsf_prepare_data_packet(crsf_packet, elrs_channels);
|
||||
ELRS_SERIAL_PORT.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){
|
||||
crsf_prepare_data_packet(crsf_packet, elrs_channels);
|
||||
ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX packet rate to 500Hz
|
||||
else if (elrs_init_counter < 505){
|
||||
crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
// Send command to update TX power to 100mW
|
||||
else if (elrs_init_counter < 510){
|
||||
crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER);
|
||||
ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE);
|
||||
elrs_init_counter++;
|
||||
}
|
||||
else{
|
||||
elrs_init_done = true;
|
||||
}
|
||||
}
|
||||
send_elrs_data();
|
||||
send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
|
||||
}
|
||||
|
||||
/* Update leds 200ms */
|
||||
if (current_timestamp >= led_timestamp)
|
||||
/* Update indicator 200ms */
|
||||
if (current_timestamp >= indicator_timestamp)
|
||||
{
|
||||
// Set led mode
|
||||
button_led_1_mode = 0;
|
||||
button_led_2_mode = 0;
|
||||
|
||||
if (joystick_calibration_mode == CALIBRATION_INIT){
|
||||
button_led_1_mode = 1;
|
||||
button_led_2_mode = 1;
|
||||
button_led_1_mode = 2;
|
||||
button_led_2_mode = 2;
|
||||
}
|
||||
else if (joystick_calibration_mode == CALIBRATION_CENTER){
|
||||
button_led_1_mode = 2;
|
||||
@ -818,77 +871,12 @@ void loop()
|
||||
}
|
||||
|
||||
/* Updated button led 1 */
|
||||
if (button_led_1_mode == 2 && button_led_1_on == false)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_1, HIGH);
|
||||
button_led_1_on = true;
|
||||
}
|
||||
else if (button_led_1_mode == 2 && button_led_1_on == true)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_1, LOW);
|
||||
button_led_1_on = false;
|
||||
}
|
||||
else if (button_led_1_mode == 1)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_1, HIGH);
|
||||
button_led_1_on = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(BUTTON_LED_1, LOW);
|
||||
button_led_1_on = false;
|
||||
}
|
||||
|
||||
update_indicator(button_led_1_mode, button_led_1_on, BUTTON_LED_1_PIN);
|
||||
/* Updated button led 2 */
|
||||
if (button_led_2_mode == 2 && button_led_2_on == false)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_2, HIGH);
|
||||
button_led_2_on = true;
|
||||
}
|
||||
else if (button_led_2_mode == 2 && button_led_2_on == true)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_2, LOW);
|
||||
button_led_2_on = false;
|
||||
}
|
||||
else if (button_led_2_mode == 1)
|
||||
{
|
||||
digitalWrite(BUTTON_LED_2, HIGH);
|
||||
button_led_2_on = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(BUTTON_LED_2, LOW);
|
||||
button_led_2_on = false;
|
||||
}
|
||||
|
||||
led_timestamp = current_timestamp + 200;
|
||||
}
|
||||
|
||||
/* Update indicator 200ms */
|
||||
if (current_timestamp >= indicator_timestamp)
|
||||
{
|
||||
update_indicator(button_led_2_mode, button_led_2_on, BUTTON_LED_2_PIN);
|
||||
/* Update status led */
|
||||
if (status_led_mode == 2 && status_led_on == false)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
}
|
||||
else if (status_led_mode == 2 && status_led_on == true)
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
}
|
||||
else if (status_led_mode == 1)
|
||||
{
|
||||
digitalWrite(STATUS_LED, HIGH);
|
||||
status_led_on = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
digitalWrite(STATUS_LED, LOW);
|
||||
status_led_on = false;
|
||||
}
|
||||
|
||||
update_indicator(status_led_mode, status_led_on, STATUS_LED_PIN);
|
||||
|
||||
indicator_timestamp = current_timestamp + 200;
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user