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

Added manually calibration values. !!! Needs to be romoved and inplemented with EEPROM !!!

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-06 19:17:33 +02:00
parent 64ddd03ad0
commit c31efd0f15
1 changed files with 31 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
rp2040/src/main.rs
View File

@ -60,7 +60,7 @@ pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
pub const GIMBAL_AXIS_RIGHT_X: usize = 2; pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
pub const GIMBAL_AXIS_RIGHT_Y: usize = 3; pub const GIMBAL_AXIS_RIGHT_Y: usize = 3;
// Create an instance with suitable settings. // Analog smoothing settings.
pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS; pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
@ -69,7 +69,6 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone, Default)]
pub struct Button { pub struct Button {
pub pressed: bool, pub pressed: bool,
pub previous_pressed: bool,
pub fn_mode: u8, pub fn_mode: u8,
} }
@ -94,7 +93,7 @@ impl Default for GimbalAxis {
min: AXIS_MIN, min: AXIS_MIN,
center: AXIS_CENTER, center: AXIS_CENTER,
fn_mode: 0, fn_mode: 0,
deadzone: (500, 50, 500), deadzone: (50, 50, 50),
expo: 0.2, expo: 0.2,
} }
} }
@ -170,6 +169,19 @@ fn main() -> ! {
// Initialize button matrix // Initialize button matrix
button_matrix.init_pins(); button_matrix.init_pins();
// Scan matrix to get initial state
for _ in 0..10 {
button_matrix.scan_matrix(&mut delay);
}
// Fallback way to enter bootloader
if button_matrix.buttons_pressed()[0] {
status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0;
rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
}
// Configure USB // Configure USB
let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new(
pac.USBCTRL_REGS, pac.USBCTRL_REGS,
@ -220,9 +232,24 @@ fn main() -> ! {
// Set up left gimbal Y axis as full range without return to center spring // Set up left gimbal Y axis as full range without return to center spring
axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN; axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN;
axis[GIMBAL_AXIS_LEFT_Y].deadzone = (500, 0, 500); axis[GIMBAL_AXIS_LEFT_Y].deadzone = (50, 0, 50);
axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0;
// Manual calibation values
// TODO: add external EEPROM and make calibration routine
axis[GIMBAL_AXIS_LEFT_X].center = AXIS_CENTER;
axis[GIMBAL_AXIS_LEFT_X].max = AXIS_MAX - 450;
axis[GIMBAL_AXIS_LEFT_X].min = AXIS_MIN + 500;
axis[GIMBAL_AXIS_LEFT_Y].center = AXIS_CENTER + 105;
axis[GIMBAL_AXIS_LEFT_Y].max = AXIS_MAX - 250;
axis[GIMBAL_AXIS_LEFT_Y].min = AXIS_MIN + 500;
axis[GIMBAL_AXIS_RIGHT_X].center = AXIS_CENTER - 230;
axis[GIMBAL_AXIS_RIGHT_X].max = AXIS_MAX - 700;
axis[GIMBAL_AXIS_RIGHT_X].min = AXIS_MIN + 350;
axis[GIMBAL_AXIS_RIGHT_Y].center = AXIS_CENTER - 68;
axis[GIMBAL_AXIS_RIGHT_Y].max = AXIS_MAX - 700;
axis[GIMBAL_AXIS_RIGHT_Y].min = AXIS_MIN + 450;
// Create dynamic smoother array for gimbal axis // Create dynamic smoother array for gimbal axis
// TODO: Find a way to store dynamic smoother in the axis struct // TODO: Find a way to store dynamic smoother in the axis struct
let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
@ -232,19 +259,6 @@ fn main() -> ! {
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
]; ];
// Scan matrix to get initial state
for _ in 0..10 {
button_matrix.scan_matrix(&mut delay);
}
// Check if first key is pressed while power on. If yes then enter bootloader
if button_matrix.buttons_pressed()[0] {
status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0;
rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
}
loop { loop {
if status_led_count_down.wait().is_ok() { if status_led_count_down.wait().is_ok() {
update_status_led(&mut status_led, &mode); update_status_led(&mut status_led, &mode);