diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 1e348c8..fa5e844 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -62,27 +62,6 @@ const XTAL_FREQ_HZ: u32 = 12_000_000u32; // | | 19 | 15 | 17 | | 24 | 20 | 22 | | // | | 18 | | 23 | | // --------------------------------------------------------------- -// Config Layer (holding CONFIG button) -// --------------------------------------------------------------- -// |BOOT L| CAL U| | CONFIG | | - | - | -// --------------------------------------------------------------- -// | | - | - | - | | - | - | - | | -// | | -// | |C M10| | - | | -// | |C M7 | | - | | -// | -/- -/- | -// | | - | | - | | -// | | - | - | - | | - |C OK | - | | -// | | - | | - | | -// --------------------------------------------------------------- -// -pub const CONFIG_BUTTON: usize = 2; -pub const BOOT_BUTTON: usize = 0; -pub const CAL_BUTTON: usize = 1; -pub const CAL_DONE_BUTTON: usize = 20; -pub const CAL_M10_GIMBLE_BUTTON: usize = 8; -pub const CAL_M7_GIMBLE_BUTTON: usize = 9; - pub const BUTTON_FRONT_LEFT_LOWER: usize = 0; pub const BUTTON_FRONT_LEFT_UPPER: usize = 1; pub const BUTTON_FRONT_LEFT_EXTRA: usize = 25; @@ -111,10 +90,15 @@ pub const BUTTON_TOP_RIGHT_HAT_RIGHT: usize = 22; pub const BUTTON_TOP_RIGHT_HAT_DOWN: usize = 23; pub const BUTTON_TOP_RIGHT_HAT_LEFT: usize = 24; +pub const USB_HAT_UP: usize = 33; +pub const USB_HAT_RIGHT: usize = 34; +pub const USB_HAT_DOWN: usize = 35; +pub const USB_HAT_LEFT: usize = 36; + // Special button functions // Throttle hold: -pub const TH_BUTTON: usize = 7; -pub const VT_BUTTON: usize = 12; +pub const TH_BUTTON: usize = BUTTON_TOP_LEFT_MODE; +pub const VT_BUTTON: usize = BUTTON_TOP_RIGHT_MODE; pub const BUTTON_ROWS: usize = 5; pub const BUTTON_COLS: usize = 5; @@ -141,6 +125,8 @@ pub const DEBOUNCE: u8 = 10; pub const RELEASE_RIMEOUT: u16 = 30; // => 300ms +pub const EEPROM_DATA_LENGTH: usize = 25; + // Public types #[derive(Copy, Clone, Default)] pub struct Button { @@ -297,9 +283,10 @@ fn main() -> ! { // Scan matrix to get initial state and check if bootloader should be entered // This is done by holding button 0 pressed while power on the unit for _ in 0..10 { + // Scan 10 times to make sure debounce routine covered all buttons button_matrix.scan_matrix(&mut delay); } - if button_matrix.buttons_pressed()[0] { + if button_matrix.buttons_pressed()[BUTTON_FRONT_LEFT_LOWER] { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; @@ -434,7 +421,7 @@ fn main() -> ! { item.center <<= 8; item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; } - gimbal_mode = eeprom.read_byte(25).unwrap(); + gimbal_mode = eeprom.read_byte(EEPROM_DATA_LENGTH as u32).unwrap(); loop { // Take care of USB HID poll requests @@ -520,8 +507,24 @@ fn main() -> ! { buttons[BUTTON_TOP_RIGHT_HAT].pressed = false; } - // Secondary way to enter bootloader (pressing all left hands buttons except the hat - if buttons[BOOT_BUTTON].pressed && buttons[CONFIG_BUTTON].pressed { + // Config Layer + // --------------------------------------------------------------- + // |BOOT L| CAL U| | CONFIG | | - | - | + // --------------------------------------------------------------- + // | | - | - | - | | - | - | - | | + // | | + // | |C M10| | - | | + // | |C M7 | | - | | + // | -/- -/- | + // | | - | | - | | + // | | - | - | - | | - |C OK | - | | + // | | - | | - | | + // --------------------------------------------------------------- + // Secondary way to enter bootloader + if buttons[BUTTON_FRONT_LEFT_LOWER].pressed + && buttons[BUTTON_TOP_LEFT_MODE].pressed + && buttons[BUTTON_TOP_RIGHT_MODE].pressed + { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; @@ -531,9 +534,11 @@ fn main() -> ! { ); } - // Calibration of center position (pressing all right hands buttons except - // the hat switch) - if buttons[CAL_BUTTON].pressed && buttons[CONFIG_BUTTON].pressed { + // Calibration of center position + if buttons[BUTTON_FRONT_LEFT_UPPER].pressed + && buttons[BUTTON_TOP_LEFT_MODE].pressed + && buttons[BUTTON_TOP_RIGHT_MODE].pressed + { for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; @@ -553,14 +558,16 @@ fn main() -> ! { } } - if calibration_active && buttons[CAL_M10_GIMBLE_BUTTON].pressed { + // Calibration set M10 gimbal mode + if calibration_active && buttons[BUTTON_TOP_LEFT_UP].pressed { gimbal_mode = GIMBAL_MODE_M10; for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; item.max = item.center; } - } else if calibration_active && buttons[CAL_M7_GIMBLE_BUTTON].pressed { + // Calibration set M7 gimbal mode + } else if calibration_active && buttons[BUTTON_TOP_LEFT_DOWN].pressed { gimbal_mode = GIMBAL_MODE_M7; for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; @@ -569,8 +576,8 @@ fn main() -> ! { } } // Save calibration data to eeprom (pressing right hat switch) - else if calibration_active && buttons[CAL_DONE_BUTTON].pressed { - let mut eeprom_data: [u8; 25] = [0; 25]; + else if calibration_active && buttons[BUTTON_TOP_RIGHT_HAT].pressed { + let mut eeprom_data: [u8; EEPROM_DATA_LENGTH] = [0; EEPROM_DATA_LENGTH]; for (index, item) in axis.iter_mut().enumerate() { eeprom_data[index * 6] = item.min as u8; eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8; @@ -579,12 +586,12 @@ fn main() -> ! { eeprom_data[(index * 6) + 4] = item.center as u8; eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8; } - eeprom_data[24] = gimbal_mode; + eeprom_data[EEPROM_DATA_LENGTH - 1] = gimbal_mode; let _ = eeprom.write_page(0x01, &eeprom_data); calibration_active = false; } - // // ON/OFF switch for Throttle hold mode + // ON/OFF switch for Throttle hold mode throttle_hold_enable = axis[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER; // Process axis values @@ -632,7 +639,7 @@ fn main() -> ! { } // Update Virtual RY - let virtual_step: u16 = 2; + let virtual_step: u16 = 5; // Compensate value when changing direction if buttons[BUTTON_FRONT_LEFT_UPPER].pressed && !buttons[BUTTON_FRONT_LEFT_LOWER].pressed @@ -712,7 +719,7 @@ fn main() -> ! { usb_activity = true; } - // Generate led activity when gimbal is moved from idle position + // Indicate activity when gimbal is moved from idle position for item in axis.iter_mut() { if item.value != item.previous_value { usb_activity = true; @@ -720,7 +727,7 @@ fn main() -> ! { item.previous_value = item.value; } - // Generate led activity when a button is pressed + // Indicate activity when a button is pressed for (index, key) in buttons.iter_mut().enumerate() { update_button_press_type(key, (timer.get_counter().ticks() / 1000) as u32); @@ -910,7 +917,7 @@ fn get_joystick_report( let mut slider: i16 = axis_12bit_to_i16(ADC_MIN); let mut hat: u8 = 8; // Hat center position - // Virtual axix control. Disables z and rx axis and using girght gimbal Y axis to control + // Virtual axix control. Disables z and rx axis and using right gimbal Y axis to control // slider axis. Values from center stick to max or min will be recalculated to min to max. if *vt_enable { if axis[GIMBAL_AXIS_RIGHT_X].value >= AXIS_CENTER { @@ -942,8 +949,8 @@ fn get_joystick_report( if key.enable_long_press { if key.active_usb_button != 0 { // Check if key is assigned as hat switch - if key.active_usb_button >= 33 && key.active_usb_button <= 36 { - hat = (key.active_usb_button as u8 - 33) * 2; + if key.active_usb_button >= USB_HAT_UP && key.active_usb_button <= USB_HAT_LEFT { + hat = (key.active_usb_button as u8 - USB_HAT_UP as u8) * 2; } else { buttons |= 1 << (key.active_usb_button - 1); } @@ -951,8 +958,8 @@ fn get_joystick_report( } else { if key.pressed && key.usb_button != 0 { // Check if key is assigned as hat switch - if key.usb_button >= 33 && key.usb_button <= 36 { - hat = (key.usb_button as u8 - 33) * 2; + if key.usb_button >= USB_HAT_UP && key.usb_button <= USB_HAT_LEFT { + hat = (key.usb_button as u8 - USB_HAT_UP as u8) * 2; } else { buttons |= 1 << (key.usb_button - 1); }