diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 9655cb6c..7cad285e 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -123,16 +123,27 @@ async fn main(main_spawner: embassy_executor::Spawner) { p ); + let mut vel: f32 = 0.0; + let mut ctr: usize = 0; loop { Timer::after_millis(100).await; + ctr += 1; + if ctr > 50 { + ctr = 0; + } else if ctr > 25 { + vel = 2.0; + } else { + vel = 0.0; + } + test_command_publisher .publish(DataPacket::BasicControl(BasicControl { _bitfield_1: Default::default(), _bitfield_align_1: Default::default(), - vel_x_linear: 1.0, + vel_x_linear: 0.0, vel_y_linear: 0.0, - vel_z_angular: 0.0, + vel_z_angular: vel * 10.0, kick_vel: 0.0, dribbler_speed: 10.0, kick_request: KickRequest::KR_DISABLE, diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index e7608f07..31e806eb 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -33,10 +33,9 @@ pub struct WheelMotor< stm32_uart_interface: Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_MOTOR_UART_QUEUES>, firmware_image: &'a [u8], - + current_timestamp_ms: u32, current_state: MotorTelemetry, current_params_state: ParameterMotorResponse, - version_major: u8, version_minor: u8, version_patch: u16, @@ -50,6 +49,8 @@ pub struct WheelMotor< motion_type: MotionCommandType::Type, reset_flagged: bool, telemetry_enabled: bool, + motion_enabled: bool, + calibrate_current: bool, } impl< @@ -81,6 +82,7 @@ impl< version_major: 0, version_minor: 0, version_patch: 0, + current_timestamp_ms: 0, current_state: start_state, current_params_state: start_params_state, vel_pid_constants: Vector3::new(0.0, 0.0, 0.0), @@ -93,6 +95,8 @@ impl< motion_type: OPEN_LOOP, reset_flagged: false, telemetry_enabled: false, + motion_enabled: false, + calibrate_current: false, } } @@ -125,6 +129,7 @@ impl< version_major: 0, version_minor: 0, version_patch: 0, + current_timestamp_ms: 0, current_state: start_state, current_params_state: start_params_state, vel_pid_constants: Vector3::new(0.0, 0.0, 0.0), @@ -137,6 +142,8 @@ impl< motion_type: OPEN_LOOP, reset_flagged: false, telemetry_enabled: false, + motion_enabled: false, + calibrate_current: false, } } @@ -216,6 +223,7 @@ impl< res = Err(()); } } + // Make sure that the uart queue is empty of any possible parameter // response packets, which may cause side effects for the flashing // process @@ -274,6 +282,16 @@ impl< return self.init_firmware_image(flash, self.firmware_image).await; } + pub async fn save_motor_current_constants(&mut self, current_constant: f32) -> Result<(), ()> { + defmt::debug!( + "Drive Motor - Saving motor current constant: {:?}", + current_constant + ); + self.stm32_uart_interface + .write_current_calibration_constants(current_constant) + .await + } + pub fn process_packets(&mut self) { while let Ok(res) = self.stm32_uart_interface.try_read_data() { let buf = res.data(); @@ -424,6 +442,18 @@ impl< self.telemetry_enabled = telemetry_enabled; } + pub fn set_motion_enabled(&mut self, enabled: bool) { + self.motion_enabled = enabled; + } + + pub fn set_calibrate_current(&mut self, calibrate_current: bool) { + self.calibrate_current = calibrate_current; + } + + pub fn read_current_timestamp_ms(&self) -> u32 { + return self.current_timestamp_ms; + } + pub fn read_is_error(&self) -> bool { return self.current_state.master_error() != 0; } @@ -434,10 +464,6 @@ impl< || self.current_state.hall_enc_vel_disagreement_error() != 0; } - pub fn read_current(&self) -> f32 { - return self.current_state.current_estimate; - } - pub fn read_encoder_delta(&self) -> i32 { return self.current_state.encoder_delta; } @@ -454,7 +480,35 @@ impl< return self.current_state.vel_setpoint; } - pub fn read_vel_computed_setpoint(&self) -> f32 { - return self.current_state.vel_computed_setpoint; + pub fn read_vel_computed_duty(&self) -> f32 { + return self.current_state.vel_computed_duty; + } + + pub fn read_current(&self) -> f32 { + return self.current_state.current_estimate; + } + + pub fn read_torque_setpoint(&self) -> f32 { + return self.current_state.torque_setpoint; + } + + pub fn read_torque_estimate(&self) -> f32 { + return self.current_state.torque_estimate; + } + + pub fn read_torque_computed_error(&self) -> f32 { + return self.current_state.torque_computed_error; + } + + pub fn read_torque_computed_nm(&self) -> f32 { + return self.current_state.torque_computed_nm; + } + + pub fn read_torque_computed_duty(&self) -> f32 { + return self.current_state.torque_computed_duty; + } + + pub fn read_vbus_voltage(&self) -> f32 { + return self.current_state.vbus_voltage; } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 50d4f90c..c3616e1b 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -29,9 +29,9 @@ use crate::{ include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} -const MAX_TX_PACKET_SIZE: usize = 60; +const MAX_TX_PACKET_SIZE: usize = 80; const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 60; +const MAX_RX_PACKET_SIZE: usize = 80; const RX_BUF_DEPTH: usize = 20; type ControlWheelMotor = @@ -378,7 +378,7 @@ impl< self.last_command.wheel_vel_control_enabled() != 0, self.last_command.wheel_torque_control_enabled() != 0, ) { - (true, true) => MotionCommandType::BOTH, + (true, true) => MotionCommandType::VELOCITY_W_TORQUE, (true, false) => MotionCommandType::VELOCITY, (false, true) => MotionCommandType::TORQUE, (false, false) => MotionCommandType::OPEN_LOOP, @@ -459,8 +459,20 @@ impl< self.motor_br.set_setpoint(wheel_vels[2]); self.motor_fr.set_setpoint(wheel_vels[3]); - // defmt::info!("wheel vels: {} {} {} {}", self.motor_fl.read_encoder_delta(), self.motor_bl.read_encoder_delta(), self.motor_br.read_encoder_delta(), self.motor_fr.read_encoder_delta()); - // defmt::info!("wheel curr: {} {} {} {}", self.motor_fl.read_current(), self.motor_bl.read_current(), self.motor_br.read_current(), self.motor_fr.read_current()); + defmt::info!( + "wheel vels: {} {} {} {}", + self.motor_fl.read_encoder_delta(), + self.motor_bl.read_encoder_delta(), + self.motor_br.read_encoder_delta(), + self.motor_fr.read_encoder_delta() + ); + defmt::info!( + "wheel curr: {} {} {} {}", + self.motor_fl.read_current(), + self.motor_bl.read_current(), + self.motor_br.read_current(), + self.motor_fr.read_current() + ); /////////////////////////////////// // send commands and telemetry // diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 4756e7df..107dd018 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -189,8 +189,7 @@ async fn imu_task_entry( } else { // After the first tipped is seen, then wait if it has been tipped for long enough. let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, first_tipped_check_time) - .unwrap() + if Instant::duration_since(&cur_time, first_tipped_check_time) .as_millis() > TIPPED_MIN_DURATION_MS { diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index ff9376a9..ce3f43c6 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -143,9 +143,7 @@ impl< } else if self.kicker_task_state >= KickerTaskState::ConnectUart { // Check if connection timeout occurred let time_elapsed = - Instant::checked_duration_since(&Instant::now(), connection_timeout_start) - .unwrap() - .as_millis(); + Instant::duration_since(&Instant::now(), connection_timeout_start).as_millis(); if time_elapsed > TELEMETRY_TIMEOUT_MS { defmt::error!("Kicker Interface - Kicker telemetry timed out, current state is '{}', rolling state back to 'Reset'", self.kicker_task_state); self.kicker_task_state = KickerTaskState::Reset; @@ -185,6 +183,8 @@ impl< ); self.kicker_driver.reset().await; self.kicker_task_state = KickerTaskState::InitFirmware; + + main_loop_ticker.reset(); } KickerTaskState::InitFirmware => { // Ensure firmware image is up to date @@ -222,6 +222,8 @@ impl< connection_timeout_start = Instant::now(); // Move on to ConnectUart state self.kicker_task_state = KickerTaskState::Connected; + + main_loop_ticker.reset(); } KickerTaskState::ConnectUart => { if telemetry_received { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 9335be5f..891d4c95 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -392,9 +392,7 @@ impl< // reboot the robot (unless we had a shutdown request). let cur_time = Instant::now(); if !cur_robot_state.shutdown_requested - && Instant::checked_duration_since(&cur_time, self.last_software_packet) - .unwrap() - .as_millis() + && Instant::duration_since(&cur_time, self.last_software_packet).as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { defmt::warn!("software timeout - rebooting..."); @@ -425,8 +423,10 @@ impl< let loop_end_time = Instant::now(); let loop_execution_time = loop_end_time - loop_start_time; - if loop_execution_time > Duration::from_millis(2) { - defmt::warn!("radio loop is taking >2ms to complete (it may be interrupted by higher priority tasks). This is >20% of an execution frame. Loop execution time {:?}", loop_execution_time); + if loop_execution_time > Duration::from_millis(2) + && self.connection_state == RadioConnectionState::Connected + { + defmt::warn!("radio loop is connected and taking >2ms to complete (it may be interrupted by higher priority tasks). This is >20% of an execution frame. Loop execution time {:?}", loop_execution_time); } last_loop_term_time = Instant::now(); diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 18e3e121..9926382e 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -59,6 +59,7 @@ async fn high_pri_kick_task( kick_pin: Peri<'static, KickPin>, chip_pin: Peri<'static, ChipPin>, mut rail_pin: Peri<'static, PowerRail200vReadPin>, + grn_led_pin: Peri<'static, GreenStatusLedPin>, err_led_pin: Peri<'static, RedStatusLedPin>, ball_detected_led_pin: Peri<'static, BlueStatusLedPin>, ) -> ! { @@ -69,8 +70,10 @@ async fn high_pri_kick_task( let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); // debug LEDs + let mut status_led = Output::new(grn_led_pin, Level::Low, Speed::Low); let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut green_led_ctr: u16 = 0; // TODO dotstars @@ -124,6 +127,17 @@ async fn high_pri_kick_task( // update telemetry requests telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; + if !telemetry_enabled { + status_led.set_high(); + } else { + if green_led_ctr > 200 { + green_led_ctr = 0; + } else if green_led_ctr > 100 { + status_led.set_low(); + } else { + status_led.set_high(); + } + } // no charge/kick in coms test let res = kick_manager @@ -131,45 +145,39 @@ async fn high_pri_kick_task( .await; // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() - .as_millis() - > 20 - { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( - res.is_err() as u16, - 0, - 0, - 0, - ball_detected as u16, - 0, - Default::default(), + let cur_time = Instant::now(); + if Instant::duration_since(&cur_time, last_packet_sent_time).as_millis() > 20 { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( + res.is_err() as u16, + 0, + 0, + 0, + ball_detected as u16, + 0, + Default::default(), + ); + kicker_telemetry_packet.rail_voltage = rail_voltage; + kicker_telemetry_packet.battery_voltage = 22.5; + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), ); - kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = 22.5; - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - if ball_detected_led.is_set_high() { - ball_detected_led.set_low(); - } else { - ball_detected_led.set_high(); - } + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } - last_packet_sent_time = cur_time; + if ball_detected_led.is_set_high() { + ball_detected_led.set_low(); + } else { + ball_detected_led.set_high(); } + + last_packet_sent_time = cur_time; } // LEDs @@ -254,6 +262,7 @@ async fn main(spawner: Spawner) -> ! { p.PD9, p.PD8, p.PC3, + p.PB9, p.PE0, p.PE1, )) diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index fac2771f..63bf1f57 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -130,13 +130,14 @@ async fn high_pri_kick_task( let mut status_led = Output::new(grn_led_pin, Level::Low, Speed::Low); let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut green_led_ctr: u16 = 0; // TODO dotstars let mut breakbeam = Breakbeam::new(breakbeam_tx.into(), breakbeam_rx.into()); // coms buffers - let mut telemetry_enabled: bool; // = false; + let mut telemetry_enabled: bool; let mut kicker_control_packet: KickerControl = Default::default(); let mut kicker_telemetry_packet: KickerTelemetry = Default::default(); let mut dribbler_motor_telemetry: MotorTelemetry = Default::default(); @@ -238,10 +239,16 @@ async fn high_pri_kick_task( // update telemetry requests telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; - if telemetry_enabled { + if !telemetry_enabled { status_led.set_high(); } else { - status_led.set_low(); + if green_led_ctr > 200 { + green_led_ctr = 0; + } else if green_led_ctr > 100 { + status_led.set_low(); + } else { + status_led.set_high(); + } } // for now shutdown requests will be latched and a reboot is required to re-power @@ -396,47 +403,41 @@ async fn high_pri_kick_task( } // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() - .as_millis() - > 20 - { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( - res.is_err() as u16, - dribbler_motor_telemetry.master_error() as u16, - shutdown_requested as u16, - shutdown_completed as u16, - ball_detected as u16, - (rail_voltage_ave > CHARGED_THRESH_VOLTAGE) as u16, - Default::default(), + let cur_time = Instant::now(); + if Instant::duration_since(&cur_time, last_packet_sent_time).as_millis() > 20 { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( + res.is_err() as u16, + dribbler_motor_telemetry.master_error() as u16, + shutdown_requested as u16, + shutdown_completed as u16, + ball_detected as u16, + (rail_voltage_ave > CHARGED_THRESH_VOLTAGE) as u16, + Default::default(), + ); + + let charge_pct = rail_voltage_ave / CHARGE_TARGET_VOLTAGE; + kicker_telemetry_packet.charge_pct = (charge_pct * 100.0) as u16; + kicker_telemetry_packet.rail_voltage = rail_voltage_ave; + kicker_telemetry_packet.battery_voltage = battery_voltage; + + kicker_telemetry_packet.dribbler_motor = dribbler_motor_telemetry; + kicker_telemetry_packet + .kicker_image_hash + .copy_from_slice(&kicker_img_hash_kicker[0..4]); + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), ); - let charge_pct = rail_voltage_ave / CHARGE_TARGET_VOLTAGE; - kicker_telemetry_packet.charge_pct = (charge_pct * 100.0) as u16; - kicker_telemetry_packet.rail_voltage = rail_voltage_ave; - kicker_telemetry_packet.battery_voltage = battery_voltage; - - kicker_telemetry_packet.dribbler_motor = dribbler_motor_telemetry; - kicker_telemetry_packet - .kicker_image_hash - .copy_from_slice(&kicker_img_hash_kicker[0..4]); - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - - last_packet_sent_time = cur_time; + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); } + + last_packet_sent_time = cur_time; } // LEDs diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index ca697fe3..1a0c7477 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -30,6 +30,11 @@ pub const STM32_BOOTLOADER_CMD_READ_PROT: u8 = 0x82; pub const STM32_BOOTLOADER_CMD_READ_UNPROT: u8 = 0x92; pub const STM32_BOOTLOADER_CMD_GET_CHECKSUM: u8 = 0xA1; +// TODO Make this shared in software-communication +pub const MOTOR_CURRENT_START_ADDRESS: u32 = 0x0800_7C00; // 512k flash, so this is the start of the last page +pub const MOTOR_CURRENT_PAGE: u8 = 31; +pub const MOTOR_CURRENT_MAGIC: [u8; 4] = [0xAA, 0xBB, 0xCC, 0xDD]; // Magic number to identify the motor current calibration data + pub fn get_bootloader_uart_config() -> Config { let mut config = usart::Config::default(); config.baudrate = 115_200; // max officially support baudrate @@ -190,7 +195,10 @@ impl< } else { defmt::debug!("bootloader replied with NACK after calibration."); } + } else { + defmt::debug!("bootloader reply too short after calibration."); } + res }), ) .await; @@ -350,8 +358,215 @@ impl< res } - pub async fn read_device_memory(&self) -> Result<(), ()> { - defmt::panic!("implement if needed."); + // Based on 3.4 of AN3155 + pub async fn read_device_memory(&self, data: &mut [u8], read_base_addr: u32) -> Result<(), ()> { + if !self.in_bootloader { + defmt::error!("called bootloader operation when not in bootloader context."); + return Err(()); + } + + let data_len = data.len(); + if data_len > 255 || data_len + 1 > LEN_TX { + defmt::error!("Data length too large for bootloader read mem command."); + return Err(()); + } + + // defmt::debug!("sending the read command..."); + self.writer + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_READ_MEM; + buf[1] = !STM32_BOOTLOADER_CMD_READ_MEM; + //defmt::info!("send buffer {:?}", buf); + 2 + }) + .await?; + + // Wait for the bootloader to acknowledge the command + let mut res = Err(()); + self.reader + .read(|buf| { + // defmt::info!("Read cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Read mem cmd replied with NACK"); + } + } else { + defmt::error!("Read mem cmd reply too short."); + } + }) + .await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the load address {:?}...", read_base_addr); + self.writer + .write(|buf| { + let start_address_bytes: [u8; 4] = read_base_addr.to_be_bytes(); + let cs = Self::bootloader_checksum_u32(read_base_addr); + buf[0] = start_address_bytes[0]; + buf[1] = start_address_bytes[1]; + buf[2] = start_address_bytes[2]; + buf[3] = start_address_bytes[3]; + buf[4] = cs; + // defmt::debug!("send buffer {:?}", buf); + 5 + }) + .await?; + + res = Err(()); + // Wait for the bootloader to acknowledge the address + self.reader + .read(|buf| { + // defmt::info!("go cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Address read mem replied with NACK"); + } + } else { + defmt::error!("Address read mem reply too short."); + } + }) + .await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the data length..."); + self.writer + .write(|buf| { + let data_len_minus_one = data_len as u8 - 1; + buf[0] = data_len_minus_one; + buf[1] = !data_len_minus_one; + // defmt::debug!("send buffer {:?}", buf); + 2 + }) + .await?; + + res = Err(()); + // defmt::debug!("reading the data..."); + self.reader + .read(|buf| { + // defmt::info!("data reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + data.copy_from_slice(&buf[1..]); + res = Ok(()); + } else { + defmt::error!("Data read mem replied with NACK"); + } + } else { + defmt::error!("Data read mem reply too short!"); + } + }) + .await?; + + res + } + + // Based on 3.8 of AN3155 + // Have to use extended erase command since the normal erase command + // doesn't seem to work? + // Designed to erase a up to N + 1 page of device memory. + pub async fn erase_device_memory_to_page( + &self, + start_page: u8, + end_page: u8, + ) -> Result<(), ()> { + if !self.in_bootloader { + defmt::error!("Called bootloader operation when not in bootloader context."); + return Err(()); + } + + if end_page < start_page { + defmt::error!("End page must be greater than or equal to start page."); + return Err(()); + } + + // defmt::debug!("sending the erase command..."); + self.writer + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + buf[1] = !STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + 2 + }) + .await?; + + let mut res = Err(()); + self.reader + .read(|buf| { + // defmt::info!("erase cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Bootloader replied to erase command with NACK"); + } + } else { + defmt::error!("Erase command reply too short."); + } + }) + .await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the page number..."); + self.writer + .write(|buf| { + // Quantity is N + 1 lead with MSB. Limited to 32 pages on STM32F1. + let erase_page_quantity = end_page - start_page; + buf[0] = 0x00; + buf[1] = erase_page_quantity; + // Need to send the page number for each page to erase in two bytes, MSB first + let mut checksum = erase_page_quantity; + // Track the index for use in returning the buffer length + let mut buf_indx: usize = 2; + for i in start_page..=end_page { + // Won't erase more than 256 pages, so always lead with 0x00 + buf[buf_indx] = 0x00; + buf[buf_indx + 1] = i; + buf_indx += 2; + // Checksum is XOR of all previous bytes. Ignore 0x00 so just LSB + checksum ^= i; + } + // Checksum for all previous bytes is just the erase page number + buf[buf_indx] = checksum; + // defmt::debug!("send buffer {:?}", buf); + // Final size is buf_indx + 1 from checksum byte + buf_indx + 1 + }) + .await?; + + // defmt::debug!("wait for erase reply"); + self.reader + .read(|buf| { + // defmt::info!("erase reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + // defmt::info!("erase accepted."); + } else { + defmt::error!("bootloader replied to erase payload with NACK"); + } + } + }) + .await?; + + res + } + + // Single page of device memory is just a page to the same page. + pub async fn erase_device_memory_single(&self, erase_page: u8) -> Result<(), ()> { + self.erase_device_memory_to_page(erase_page, erase_page) + .await } async fn write_device_memory_chunk(&self, data: &[u8], write_base_addr: u32) -> Result<(), ()> { @@ -463,7 +678,7 @@ impl< return Err(()); } - // ensure step size is below bootlaoder supported, below transmit buffer size (incl len and cs bytes), + // ensure step size is below bootloader supported, below transmit buffer size (incl len and cs bytes), // and is 4-byte aligned let step_size = min(256, LEN_TX - 2) & 0xFFFF_FFF8; defmt::debug!("bootloader will use data chunk sizes of {:?}", step_size); @@ -589,6 +804,111 @@ impl< Ok(()) } + pub async fn load_motor_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> { + if !self.in_bootloader { + if let Err(err) = self.reset_into_bootloader().await { + return Err(err); + } + } + + if let Err(err) = self.verify_bootloader().await { + return Err(err); + } + + match self.get_device_id().await { + Err(err) => return Err(err), + Ok(device_id) => match device_id { + 68 => { + defmt::trace!("found stm32f1 device"); + } + 19 => { + defmt::trace!("found stm32f40xxx device"); + } + 105 => { + defmt::trace!("found stm32g474xx device"); + } + _ => { + defmt::error!("found unknown device id {}", device_id); + return Err(()); + } + }, + }; + + // Erase up to Page 31 since the last page is used for current calibration constants. + if let Err(err) = self.erase_device_memory_to_page(0, 15).await { + return Err(err); + } + + // Split up to 2 commands to reduce UART packet size. + if let Err(err) = self + .erase_device_memory_to_page(16, MOTOR_CURRENT_PAGE) + .await + { + return Err(err); + } + + // program image + if let Err(err) = self.write_device_memory(fw_image_bytes, None).await { + return Err(err); + } + + self.reset_into_program(true).await; + + Ok(()) + } + + pub async fn write_current_calibration_constants( + &mut self, + current_constant: f32, + ) -> Result<(), ()> { + if !self.in_bootloader { + if let Err(err) = self.reset_into_bootloader().await { + return Err(err); + } + } + + if let Err(err) = self.verify_bootloader().await { + return Err(err); + } + + match self.get_device_id().await { + Err(err) => return Err(err), + Ok(device_id) => match device_id { + 68 => { + defmt::trace!("found STSPINF0 device"); + } + _ => { + defmt::error!( + "Invalid device id for current calibration constants {}", + device_id + ); + return Err(()); + } + }, + }; + + // Erase the last page + if let Err(err) = self.erase_device_memory_single(MOTOR_CURRENT_PAGE).await { + return Err(err); + } + + // Write the constants to the last page but with the magic number prepended + let mut data_to_write = [0u8; 8]; + data_to_write[..4].copy_from_slice(&MOTOR_CURRENT_MAGIC); + // Convert the f32 to bytes and write it after the magic number + let current_bytes = current_constant.to_le_bytes(); + data_to_write[4..8].copy_from_slice(¤t_bytes); + + if let Err(err) = self + .write_device_memory(&data_to_write, Some(MOTOR_CURRENT_START_ADDRESS)) + .await + { + return Err(err); + } + + Ok(()) + } + pub async fn execute_code(&mut self, start_address: Option) -> Result<(), ()> { defmt::warn!("firmware jump/go command implementation does not work on some parts. If this appears not to work, consider simply resetting the device."); diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 6a3d74b4..eb18e3a2 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -59,8 +59,11 @@ int main() { bool telemetry_enabled = false; // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } // initialize motor driver pwm6step_setup(); @@ -104,6 +107,7 @@ int main() { // Turn off Red/Yellow LED after booting. turn_off_red_led(); turn_off_yellow_led(); + uint16_t green_led_ctr = 0; #ifdef UART_ENABLED // Initialize UART and logging status. @@ -212,7 +216,7 @@ int main() { bool run_telemetry = time_sync_ready_rst(&telemetry_timer); if (run_torque_loop) { - float cur_measurement = ((float) res.I_motor / (float) UINT16_MAX) * AVDD_V; + float cur_measurement = currsen_get_motor_current_with_offset(); // TODO: recover current from voltage // TODO: estimate torque from current // TODO: filter? @@ -229,7 +233,7 @@ int main() { response_packet.data.motion.torque_setpoint = r; response_packet.data.motion.torque_estimate = cur_measurement; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint; + response_packet.data.motion.torque_computed_nm = torque_setpoint; } if (run_torque_loop) { @@ -289,7 +293,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -358,7 +362,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } @@ -371,17 +375,25 @@ int main() { turn_off_yellow_led(); } - // Green LED means we are able to send telemetry upstream. - // This means the upstream sent a packet downstream with telemetry enabled. + // Green LED means system is up. Flicker means we're acknowledging that + // control has enabled the system (non-zero motor commands allowed) if (!telemetry_enabled) { - turn_off_green_led(); - } else { turn_on_green_led(); + } else { + // 5Hz flicker + if (green_led_ctr > 200) { + green_led_ctr = 0; + } else if (green_led_ctr > 100) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + green_led_ctr++; } #ifdef COMP_MODE - if (telemetry_enabled && - ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { // If have telemetry enabled (meaning we at one // point received a message from upstream) and haven't // received a command packet in a while, we need to reset diff --git a/motor-controller/bin/dribbler/main.h b/motor-controller/bin/dribbler/main.h index 2e1149ac..c5884915 100644 --- a/motor-controller/bin/dribbler/main.h +++ b/motor-controller/bin/dribbler/main.h @@ -15,8 +15,6 @@ // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 - #define AVDD_V 3.3f /////////////////////////////////////////// diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h index d0427c03..d26b06df 100644 --- a/motor-controller/bin/dribbler/system.h +++ b/motor-controller/bin/dribbler/system.h @@ -16,7 +16,7 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c index 45e78ae3..8b8445aa 100644 --- a/motor-controller/bin/wheel-test/main.c +++ b/motor-controller/bin/wheel-test/main.c @@ -62,8 +62,11 @@ int main() { bool telemetry_enabled = false; // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } // initialize motor driver pwm6step_setup(); @@ -293,9 +296,8 @@ int main() { if (run_torque_loop) { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V - float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; // map voltage given by the amp network to current - float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = currsen_get_motor_current_with_offset(); // map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); // filter torque @@ -336,7 +338,7 @@ int main() { response_packet.data.motion.current_estimate = current_sense_I; response_packet.data.motion.torque_estimate = measured_torque_Nm; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm; + response_packet.data.motion.torque_computed_nm = torque_setpoint_Nm; } // run velocity loop if applicable @@ -368,11 +370,11 @@ int main() { // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_setpoint_clamped = control_setpoint_vel_rads; + response_packet.data.motion.vel_computed_rads = control_setpoint_vel_rads; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; + response_packet.data.motion.vel_computed_duty = control_setpoint_vel_duty; } if (run_torque_loop || run_vel_loop) { @@ -428,7 +430,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -458,7 +460,7 @@ int main() { response_packet.data.params.version_major = VERSION_MAJOR; response_packet.data.params.version_major = VERSION_MINOR; response_packet.data.params.version_major = VERSION_PATCH; - response_packet.data.params.timestamp = time_local_epoch_s(); + response_packet.timestamp = time_local_epoch_s(); response_packet.data.params.vel_p = vel_pid_constants.kP; response_packet.data.params.vel_i = vel_pid_constants.kI; @@ -500,7 +502,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h index c89e19cf..178e00ef 100644 --- a/motor-controller/bin/wheel-test/main.h +++ b/motor-controller/bin/wheel-test/main.h @@ -21,7 +21,6 @@ // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 #define AVDD_V 3.3f diff --git a/motor-controller/bin/wheel-test/system.h b/motor-controller/bin/wheel-test/system.h index 0d5d9a17..ab709741 100644 --- a/motor-controller/bin/wheel-test/system.h +++ b/motor-controller/bin/wheel-test/system.h @@ -18,7 +18,7 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 03d58be3..7ffb0635 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -57,21 +57,28 @@ int main() { // Setups clocks setup(); + // setup encoder + quadenc_setup(); + quadenc_reset_encoder_delta(); + + // initialize current sensing setup + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } + // start watchdog IWDG->KR = 0x0000CCCC; // enable the module IWDG->KR = 0x00005555; // enable register writes IWDG->PR = 0x4; // set prescaler to 64, 40kHz -> 625Hz, 1.6ms per tick - IWDG->RLR = 5; // count to 10 ticks, 16ms then trigger a system reset + IWDG->RLR = 10; // count to 10 ticks, 16ms then trigger a system reset while (IWDG->SR) {} // wait for value to take IWDG->KR = 0x0000AAAA; // feed the watchdog uint32_t ticks_since_last_command_packet = 0; bool telemetry_enabled = false; - // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); - // initialize motor driver pwm6step_setup(); pwm6step_set_duty_cycle_f(0.0f); @@ -79,9 +86,13 @@ int main() { // enable ADC hardware trigger (tied to 6step timer) currsen_enable_ht(); - // setup encoder - quadenc_setup(); - quadenc_reset_encoder_delta(); + wait_ms(5); + + // calibrate current + while (!currsen_calibrate_sense()) { + wait_ms(1); + IWDG->KR = 0x0000AAAA; // feed the watchdog + } // Initialized response_packet here to capture the reset method. MotorResponse response_packet; @@ -195,6 +206,8 @@ int main() { turn_off_red_led(); turn_off_yellow_led(); + uint16_t green_led_ctr = 0; + #ifdef UART_ENABLED // Initialize UART and logging status. uart_initialize(); @@ -330,9 +343,14 @@ int main() { if (run_torque_loop) { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V - float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; + float vbatt = currsen_get_vbus_voltage(); // map voltage given by the amp network to current - float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = currsen_get_motor_current(); // mm_voltage_to_current(&df45_model, current_sense_shunt_v); + current_sense_I *= (0.25f / currsen_get_motor_current_offset()); + // current_sense_I = currsen_get_motor_current_offset(); + // current_sense_I = currsen_get_shunt_voltage_raw(); + // float current_sense_I = current_sense_shunt_v; + // float current_sense_I = current_sense_shunt_v; // map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); // filter torque @@ -373,7 +391,7 @@ int main() { response_packet.data.motion.current_estimate = current_sense_I; response_packet.data.motion.torque_estimate = measured_torque_Nm; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm; + response_packet.data.motion.torque_computed_nm = torque_setpoint_Nm; } // run velocity loop if applicable @@ -414,11 +432,11 @@ int main() { // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_setpoint_clamped = control_setpoint_vel_rads; + response_packet.data.motion.vel_computed_rads = control_setpoint_vel_rads; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; + response_packet.data.motion.vel_computed_duty = control_setpoint_vel_duty; } if (run_torque_loop || run_vel_loop) { @@ -430,7 +448,7 @@ int main() { if (motion_control_type == OPEN_LOOP) { float r_motor = mm_rads_to_dc(&df45_model, r_motor_board); response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_computed_setpoint = r_motor; + response_packet.data.motion.vel_setpoint = r_motor; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); @@ -487,7 +505,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -517,7 +535,7 @@ int main() { response_packet.data.params.version_major = VERSION_MAJOR; response_packet.data.params.version_minor = VERSION_MINOR; response_packet.data.params.version_patch = VERSION_PATCH; - response_packet.data.params.timestamp = time_local_epoch_s(); + response_packet.timestamp = time_local_epoch_s(); // TODO parameter updates are off for gain scheduled PID // response_packet.data.params.vel_p = vel_pid_constants.kP; @@ -562,7 +580,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } @@ -575,17 +593,25 @@ int main() { turn_off_yellow_led(); } - // Green LED means we are able to send telemetry upstream. - // This means the upstream sent a packet downstream with telemetry enabled. + // Green LED means system is up. Flicker means we're acknowledging that + // control has enabled the system (non-zero motor commands allowed) if (!telemetry_enabled) { - turn_off_green_led(); - } else { turn_on_green_led(); + } else { + // 5Hz flicker + if (green_led_ctr > 200) { + green_led_ctr = 0; + } else if (green_led_ctr > 100) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + green_led_ctr++; } #ifdef COMP_MODE - if (telemetry_enabled && - ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { // If have telemetry enabled (meaning we at one // point received a message from upstream) and haven't // received a command packet in a while, we need to reset diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h index 63baec1e..b29471c4 100644 --- a/motor-controller/bin/wheel/main.h +++ b/motor-controller/bin/wheel/main.h @@ -21,9 +21,8 @@ // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 -#define AVDD_V 3.3f +#define AVDD_V 3.0f /////////////////////////////////////////// // current sense amplification network // diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 4c6c1c1c..09799006 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -18,7 +18,7 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// @@ -26,9 +26,14 @@ ////////////////// #define ADC_MODE CS_MODE_DMA -#define ADC_NUM_CHANNELS 4 -#define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17) -#define ADC_SR_MASK (ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) +#define ADC_NUM_CHANNELS 2 +// #define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17) +#define ADC_CH_MASK (ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL9) +// #define ADC_CH_MASK (ADC_CHSELR_CHSEL4) +// #define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1) +// #define ADC_SR_MASK (ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) +#define ADC_SR_MASK (ADC_SMPR_SMP_0) +// #define ADC_SR_MASK 0 //////////////////// // TIME KEEPING // diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 98d4ee6c..1b00f4b2 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -677,15 +677,18 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_low = commutation_values[4]; bool phase3_high = commutation_values[5]; - uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) 48000 * (PWM_TIM_PRESCALER + 1)) - 1); - TIM1->CCR4 = arr_pwm_dc_value; + // uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); + // TIM1->CCR4 = arr_pwm_dc_value; // uint16_t ccer = 0; uint16_t ccer = CCER_TIM4_ADC_TRIG; uint16_t ccmr1 = 0; // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; - TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + // TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + + TIM1->CCR4 = 22; + if (phase1_low) { if (command_brake) { TIM1->CCR1 = current_duty_cycle; diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index e32acce9..d305373d 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -17,10 +17,10 @@ #define MAX_DUTYCYCLE_COMMAND 65535U #define MIN_DUTYCYCLE_COMMAND -(MAX_DUTYCYCLE_COMMAND) -#define NUM_RAW_DC_STEPS 1000U -#define MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW 10U -#define SCALING_FACTOR ((MAX_DUTYCYCLE_COMMAND / (NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW)) + 1U) -#define MAP_UINT16_TO_RAW_DC(dc) (dc > 0 ? ((dc / SCALING_FACTOR) + MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW) : 0U) +// period 20833ns +// be conscious of dead time ratio +#define PWM_TIM_PRESCALER 0 +#define PWM_FREQ_HZ 40000 ////////////////////// // ERROR HANDLING // @@ -49,12 +49,6 @@ typedef struct MotorErrors { // LOW LEVEL CONTROL PARAMS // //////////////////////////////// -#define PWM_TIM_PRESCALER 0 - -// period 20833ns -// be conscious of dead time ratio -#define PWM_FREQ_HZ 48000 - // no div on CK_INT = 48MHz // Tdts = CK_INT = 48MHz // xxx yyyyy -> 0xx selects no multiplier @@ -64,6 +58,13 @@ typedef struct MotorErrors { // 0000 0111 = 0x07 #define DEAD_TIME 0x07 +#define NUM_RAW_DC_STEPS (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1))) +#define MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW 10U +#define SCALING_FACTOR ((MAX_DUTYCYCLE_COMMAND / (NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW)) + 1U) +#define MAP_UINT16_TO_RAW_DC(dc) (dc > 0 ? ((dc / SCALING_FACTOR) + MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW) : 0U) + +// #define FORCE_COMUTATION_TRANSITION_DELAY + //////////////////////// // PUBLIC FUNCTIONS // //////////////////////// diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index afbf0c22..16ac7264 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -1,12 +1,11 @@ /** * @file main.c - * @author your name (you@domain.com) - * @brief + * @author Joe Spall + * @brief * @version 0.1 - * @date 2022-04-10 - * - * @copyright Copyright (c) 2022 - * + * + * @copyright Copyright (c) 2025 + * */ #include @@ -15,156 +14,191 @@ #include "current_sensing.h" #include "time.h" - //////////////////////////// /// ADC Channel 4 -> PA4 /// //////////////////////////// -static CS_Mode_t cs_mode; +static CS_Mode_t m_cs_mode; +static ADC_Result_t m_adc_result; +static bool m_adc_calibrated = false; + +static bool m_motor_adc_offset_set = false; +static size_t m_motor_adc_offset_ctr = 0; +static float m_motor_adc_offset = 0.0f; void currsen_enable_ht() { ADC1->CR |= ADC_CR_ADSTART; } void currsen_read_dma() { - // GPIOB->BSRR |= GPIO_BSRR_BS_9; ADC1->CR |= ADC_CR_ADSTART; } void DMA1_Channel1_IRQHandler() { - // GPIOB->BSRR |= GPIO_BSRR_BR_9; DMA1->IFCR |= DMA_IFCR_CGIF1; } -/** - * @brief run current sense - * - * @return ADC_Result result of the ADC operation - */ -void currsen_read(ADC_Result_t *res) -{ - const int NUM_CHANNELS = 4; - - ADC1->CR |= ADC_CR_ADSTART; - - //ADC_Result_t res; - res->status = CS_OK; - while (1) - { - - for (int i = 0; i < NUM_CHANNELS; i++) - { - // Start ADC conversion - ADC1->CR |= ADC_CR_ADSTART; - - // Wait until end of conversion - uint32_t timeout_count = 0; - while ((ADC1->ISR & ADC_ISR_EOC) == 0) - { - if (timeout_count < ADC_DIS_TIMEOUT) - { - //wait_ms(1); - //timeout_count += 1; - } - else - { - res->status = CS_TIMEOUT; - //return res; - } - } - // Store the ADC conversion - uint16_t currADC = ADC1->DR; - switch (i) - { - case 0: - res->I_motor_filt = currADC; - //return; - break; - case 1: - res->I_motor = currADC; - break; - case 2: - res->T_spin = currADC; - break; - case 3: - res->V_int = currADC; - return; - //return res; - } - } - } - -} - - /** * @brief configure, calibrate, and enable the ADC - * + * * @return CS_Status_t status of the operation */ -CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uint32_t ch_sel, uint32_t sr_sel) +CS_Status_t currsen_setup(uint8_t motor_adc_ch) { - cs_mode = mode; - - memset(res, 0, sizeof(ADC_Result_t)); + memset(&m_adc_result, 0, sizeof(ADC_Result_t)); // Assume ADC has not been set up yet CS_Status_t status = CS_OK; - // pre config dma if enabled - if (cs_mode == CS_MODE_DMA || cs_mode == CS_MODE_TIMER_DMA) { - DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); - DMA1_Channel1->CMAR = (uint32_t) res; - DMA1_Channel1->CNDTR = num_ch; - DMA1_Channel1->CCR |= (DMA_CCR_MINC | DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0 | DMA_CCR_TEIE | DMA_CCR_CIRC); - DMA1_Channel1->CCR |= DMA_CCR_EN; - - // DMA1->IFCR |= DMA_IFCR_CGIF1; - // DMA1_Channel1->CCR |= DMA_CCR_TCIE; - - // NVIC_SetPriority(DMA1_Ch1_IRQn, 5); - // NVIC_EnableIRQ(DMA1_Ch1_IRQn); - } - - // Configuration - status = currsen_adc_conf(); + // Disable ADC before configuration. + status = currsen_adc_dis(); if (status != CS_OK) return status; - // Group Configuration - status = currsen_adc_group_config(); - - // Calibration + // DMA Setup + // Disable DMA1 Channel 1 before configuring. + DMA1_Channel1->CCR &= ~DMA_CCR_EN; + // Set DMA to 16-bit memory size (0b01) + DMA1_Channel1->CCR |= DMA_CCR_MSIZE_0; + DMA1_Channel1->CCR &= ~DMA_CCR_MSIZE_1; + // Set DMA to 16-bit peripheral size (0b01) + DMA1_Channel1->CCR |= DMA_CCR_PSIZE_0; + DMA1_Channel1->CCR &= ~DMA_CCR_PSIZE_1; + // Set DMA to circular mode + DMA1_Channel1->CCR |= DMA_CCR_CIRC; + // Increment memory address + DMA1_Channel1->CCR |= DMA_CCR_MINC; + // Set DMA Channel 1 transfer error interrupt enable. + DMA1_Channel1->CCR |= DMA_CCR_TEIE; + // Set DMA Channel 1 half transfer interrupt and transfer complete interrupt to disable. + DMA1_Channel1->CCR &= ~(DMA_CCR_HTIE | DMA_CCR_TCIE); + // Set DMA Channel 1 direction to read from peripheral. + DMA1_Channel1->CCR &= ~DMA_CCR_DIR; + // Set DMA Channel 1 priority to very high. + DMA1_Channel1->CCR |= DMA_CCR_PL; + + // Set DMA Channel 1 Peripheral Address to ADC1 Data Register. + DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); + // Set DMA Channel 1 Memory Address to the result struct. + DMA1_Channel1->CMAR = (uint32_t) (&m_adc_result); + // Set DMA Channel 1 Number of Data to Transfer to the number of transfers. + // Motor and Vbus, so 2 transfers. + // Since in circular mode, this will reset. + DMA1_Channel1->CNDTR = 2; + // Enable DMA1 Channel 1. + DMA1_Channel1->CCR |= DMA_CCR_EN; + + // Calibration (should happen before DMAEN in CFGR1) status = currsen_adc_cal(); if (status != CS_OK) return status; + // Set ADC data resolution + // Based on Table 46 of RM0091. + // 12 bit has a t_SAR = 12.5 ADC clock cycles (have margin to do so below). + // Set ADC conversion data alignment - left align + MODIFY_REG( + ADC1->CFGR1, + ADC_CFGR1_RES | + ADC_CFGR1_ALIGN, + ADC_RESOLUTION_12B | + ADC_DATA_ALIGN_RIGHT); + + // Set ADC low power mode - None + MODIFY_REG( + ADC1->CFGR1, + (ADC_CFGR1_WAIT | ADC_CFGR1_AUTOFF), + ADC_LP_MODE_NONE); + + // Set ADC clock + // Based on Table 44 of RM0091. + // PCLK DIV 4, Latency on starting sample is deterministic. + // t_LATENCY = 2.625 ADC clock cycles. + // PCLK (48 MHz) / 4 = 12 MHz ADC clock. + // Must be DIV 4 for 48 MHz PCLK to be less than 14 MHz ADC clock. + // NOTE: ST does clock async CKMODE = 00. We are not doing the + // fastest (14 MHz > 12 MHz) but there is no jitter on start timing. + // From Table 50 of stm32f031c4.pdf + // W_LATENCY = 8.5 ADC clock cycles for PCLK/4. + MODIFY_REG( + ADC1->CFGR2, + ADC_CFGR2_CKMODE, + ADC_CFGR2_CKMODE_1); + + // TIM1 Ch4 Falling Edge trigger + // TIM1 -> Triggering 48 kHz + // 1 / 48 kHz = 20.833 us period + // Falling edge is used because the low-side gate should be closed + // before the ADC conversion starts. + // Not discontinuous sampling. + // Single conversion. + // Unlimited DMA transfers. + // Overwrite data on conversion. + MODIFY_REG(ADC1->CFGR1, + ADC_CFGR1_EXTSEL + | ADC_CFGR1_EXTEN + | ADC_CFGR1_DISCEN + | ADC_CFGR1_CONT + | ADC_CFGR1_DMAEN + | ADC_CFGR1_DMACFG + | ADC_CFGR1_OVRMOD + , + ADC_REG_TRIG_EXT_TIM1_CH4 + | ADC_REG_SEQ_DISCONT_DISABLE + | ADC_REG_CONV_SINGLE + | ADC_REG_DMA_TRANSFER_UNLIMITED + | ADC_REG_OVR_DATA_OVERWRITTEN); + + /* + FUTURE: If doing temperature, need sampling of ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1. + Motors need 0, so need to switch sampling duration on the fly so would require + DMA + ADC trigger. + + // Temperature sensor values + // Table 53. STM32F031x6.pdf + // ts_temp minimum ADC sampling time -> 4 us + // At 12 MHz ADC clock -> 48 ADC clock cycles. + // SMP must be 101 (55.5 ADC clock cycles) to 111 (239.5) for temperature sensor. + // Wake-up the Temperature Sensor + ADC->CCR |= (ADC_CCR_TSEN); + + // Since sampling + conversion period must be less than + // 20.833 us, we need to set the sampling time to be less, so + // SMP must be less than 111 (239.5 ADC clock cycles). + + // Total sampling + conversion time + // From Figure 31 of RM0091 + // Total first conversion -> (t_LATENCY + t_SMPL + t_SAR + W_LATENCY) + // All following conversions -> (t_SMPL + t_SAR + W_LATENCY) + // t_SMPL -> based on SMP[2:0] bits. Min is 1.5 and max is 239.5 ADC clock cycles (13.11.6 in RM0091). + // SMP = 110 (71.5 ADC clock cycles) seems to be the minimum for temperature sensor. + // Total first conversion -> (2.625 + 71.5 + 12.5 + 8.5) = 95.125 ADC clock cycles. + // first conversion * (1 / 12 MHz ADC Clock) = 7.92708 us for 1 channel. + // All following conversions -> (71.5 + 12.5 + 8.5) = 92.5 ADC clock cycles. + // following conversions * (1 / 12 MHz ADC Clock) = 7.7083 us for each additional. + // So if we do one motor and temp sensor, we are at 7.92708 + 7.7083 = 15.63538 us. + // FUTURE ^ This is not enough time I think so got to do the on the fly sampling. + ADC1->SMPR = ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1; + + // Set ADC channel selection + // Channel 16 is the temperature sensor. + ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL16; + */ + + // 7.5 ADC clock cycles (0.625 us) is the minimum sampling time for the motor. + // T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us + ADC1->SMPR = ADC_SMPR_SMP_0; + // ADC1->SMPR = 0; + + + // Set ADC channel selection. Ch9 is the Vbus. + ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL9; // Enable status = currsen_adc_en(); if (status != CS_OK) return status; - // Select ch 3 (pot), ch 4 (motor pin), ch5 (Vbat), ch16 (Temp) ch 17 (Vref), ch 18 (Vbat - internal spec not Vbus motor) - // in deployed firmware don't check pot, convert motor first - //ADC1->CHSELR = ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL5 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17; // | ADC_CHSELR_CHSEL18; - ADC1->CHSELR = ch_sel; - // ADC1->CHSELR = ADC_CHSELR_CHSEL3; - // Select a sampling mode of 111 i.e. 239.5 ADC clk to be greater than 17.1us - // at 10B rest 11.5 cycles + (sm 111)=239.5 = 251 cycles - // useing PCLK / 2 = 48MHz / 2 = 24MHz = 1 / 24_000_000 = 0.000_000_041 ADC clk - // 251 cycles -> 10.2 us conversion (max precision) - // - // PWM @48kHz = period 20.8uS - // have a max of 5 conversions, < 4us per conversion - // pick sm 110 = 71.5 clock cycles - // total conversion = 11.5 + 71.5 = 83 cyc - // 83 cyc * 0.000_000_041 = 3.4us per sample, 3.4 * 5 = 17us - // default 3 cycle + sample cycles * ADC clk 14MHz 0.000_000_071 s per cycle; - // TODO Need to configure timing based on rise resistance - //ADC1->SMPR |= ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2; - ADC1->SMPR = sr_sel; - // fires interrupt on end of sequence to drop GPIOB, pin 9 // used for current sampling/timing verification // ADC1->IER |= (ADC_IER_EOSEQIE); @@ -175,92 +209,23 @@ CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uin return status; } -void ADC1_IRQHandler() { - GPIOB->BSRR |= GPIO_BSRR_BR_9; - ADC1->ISR |= (ADC_ISR_EOSEQ); -} - -/** - * @brief configure the ADC group - * - * @return CS_Status_t status of the operation - */ -CS_Status_t currsen_adc_group_config() -{ - // Software trigger - // Not discontinuous sampling - // Continuous conversion - // No DMA - // Overwrite data - MODIFY_REG(ADC1->CFGR1, - ADC_CFGR1_EXTSEL - | ADC_CFGR1_EXTEN - | ADC_CFGR1_DISCEN - | ADC_CFGR1_CONT - | ADC_CFGR1_DMAEN - | ADC_CFGR1_DMACFG - | ADC_CFGR1_OVRMOD - , - ADC_REG_TRIG_EXT_TIM1_CH4 - // ((cs_mode == CS_MODE_TIMER_DMA) ? ADC_REG_TRIG_EXT_TIM1_TRGO : ADC_REG_TRIG_SOFTWARE) - | ADC_REG_SEQ_DISCONT_DISABLE //| ADC_REG_CONV_CONTINUOUS - | ADC_REG_CONV_SINGLE - | ADC_REG_DMA_TRANSFER_UNLIMITED // | ADC_REG_DMA_TRANSFER_NONE - | ADC_REG_OVR_DATA_OVERWRITTEN - ); - - // Wake-up the VREFINT (only for VBAT, Temp sensor and VRefInt) - ADC->CCR |= ADC_CCR_VREFEN; // enable internal Vref source - ADC->CCR |= ADC_CCR_TSEN; // enable internal temp source - - return CS_OK; -} - - -/** - * @brief configure the ADC - * - * @return CS_Status_t status of the operation - */ -CS_Status_t currsen_adc_conf() -{ - // Assume ADC not enabled - // TODO check if ADC enabled - - // Set ADC data resolution - // 10 bit, 11.5 t_SAR, 1.5 t_SMPL = 13 t_CONV clock cycles - // Set ADC conversion data alignment - left align - // Set ADC low power mode - None - MODIFY_REG(ADC1->CFGR1, - ADC_CFGR1_RES - | ADC_CFGR1_ALIGN - | ADC_CFGR1_WAIT - | ADC_CFGR1_AUTOFF - , - ADC_RESOLUTION_10B - | ADC_DATA_ALIGN_LEFT - | ADC_LP_MODE_NONE - ); - - // Set ADC clock - // PCLK DIV 2, Latency is deterministic (no jitter) and equal to - // 2.75 ADC clock cycles - MODIFY_REG(ADC1->CFGR2, - ADC_CFGR2_CKMODE - , - ADC_CFGR2_CKMODE_0 - ); - - return CS_OK; -} +//void ADC1_IRQHandler() { +// GPIOB->BSRR |= GPIO_BSRR_BR_9; +// ADC1->ISR |= (ADC_ISR_EOSEQ); +//} /** * @brief calibrate the ADC - * + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_cal() { + if (m_adc_calibrated) { + // If already calibrated, return OK. + return CS_OK; + } + // Ensure that ADEN = 0 before calibration if ((ADC1->CR & ADC_CR_ADEN) != 0) { @@ -283,10 +248,10 @@ CS_Status_t currsen_adc_cal() } } - // Clear DMAEN - ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN; - // Launch the calibration by setting ADCAL - ADC1->CR |= ADC_CR_ADCAL; + // Clear DMAEN just to be safe. + ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN; + // Launch the calibration by setting ADCAL + ADC1->CR |= ADC_CR_ADCAL; // Waiting for calibration to finish, timeout if too long timeout_count = 0; @@ -303,30 +268,28 @@ CS_Status_t currsen_adc_cal() } } + m_adc_calibrated = true; + return CS_OK; } - /** * @brief enables the ADC - * + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_en() -{ +{ // Check if ADC is already powered up, and clear if so if ((ADC1->ISR & ADC_ISR_ADRDY) != 0) { - // Clear ADRDY + // Clear ADRDY ADC1->ISR |= ADC_ISR_ADRDY; } // Enable the ADC ADC1->CR |= ADC_CR_ADEN; - if (cs_mode == CS_MODE_DMA || cs_mode == CS_MODE_TIMER_DMA) { - ADC1->CFGR1 |= ADC_CFGR1_DMAEN; - } - + // Waiting for ADC to be powered up, timeout if too long uint32_t timeout_count = 0; while ((ADC1->ISR & ADC_ISR_ADRDY) == 0) @@ -342,20 +305,31 @@ CS_Status_t currsen_adc_en() } } + // Enable the DMA after the ADC is powered up. + ADC1->CFGR1 |= ADC_CFGR1_DMAEN; + return CS_OK; } /** - * @brief disables the ADC - * + * @brief disables the ADC. Procedure pulled directly from RM0091 page 238. + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_dis() -{ - // Stop any ongoing conversion +{ + // Check if ADC is already disabled + // (not enabled and not converting). + if ((ADC1->CR & ADC_CR_ADEN) == 0 && + (ADC1->CR & ADC_CR_ADSTART) == 0) + { + return CS_OK; + } + + // Stop any ongoing conversion. ADC1->CR |= ADC_CR_ADSTP; - // Waiting until conversion is stopped + // Waiting until conversion is stopped. uint32_t timeout_count = 0; while ((ADC1->CR & ADC_CR_ADSTP) != 0) { @@ -372,7 +346,7 @@ CS_Status_t currsen_adc_dis() // Disable the ADC ADC1->CR |= ADC_CR_ADDIS; - // Waiting until ADC is fully disabled + // Waiting until ADC is fully disabled. timeout_count = 0; while ((ADC1->CR & ADC_CR_ADEN) != 0) { @@ -388,4 +362,119 @@ CS_Status_t currsen_adc_dis() } return CS_OK; +} + +float currsen_get_shunt_voltage_raw() { + float v_adc = ((float) (m_adc_result.Motor_current_raw)) * V_ADC_SCALE_V; + + return v_adc; +} + +float currsen_get_shunt_voltage() { + float v_motor_sense = (currsen_get_shunt_voltage_raw() - currsen_get_motor_current_offset()) / MOTOR_OPAMP_GAIN_REAL; + if (v_motor_sense < 0.0f) { + v_motor_sense = 0.0f; + } + + return v_motor_sense; +} + +/** + * @brief gets the motor current. Translate from raw ADC value to + * to scaled raw ADC value to motor current. + * + * @return float motor current + */ +float currsen_get_motor_current() +{ + float i_motor = currsen_get_shunt_voltage() / MOTOR_OPAMP_RESISTOR_SENSE; + + return i_motor; +} + +float currsen_get_motor_current_offset() +{ + return m_motor_adc_offset; +} + +float currsen_get_motor_current_with_offset() +{ + // Get the current in amps from the current sense ADC. + // Should always be positive with the current electrical architecture. + float i_motor = currsen_get_motor_current(); + // Apply the offset to the motor current. + i_motor -= currsen_get_motor_current_offset(); + + if (i_motor < 0.0f) { + // If the current is negative, set it to 0.0f. + // This is to prevent negative current readings. + i_motor = 0.0f; + } + + return i_motor; +} + +bool currsen_calibrate_sense() +{ + if (m_motor_adc_offset_set) { + m_motor_adc_offset_set = false; + m_motor_adc_offset_ctr = 0; + m_motor_adc_offset = 0.0f; + } + + m_motor_adc_offset += currsen_get_shunt_voltage_raw(); + m_motor_adc_offset_ctr++; + + if (m_motor_adc_offset_ctr >= 10) { + m_motor_adc_offset /= 10.0f; + m_motor_adc_offset_set = true; + } + + return m_motor_adc_offset_set; +} + +/** + * @brief gets the Vbus voltage. Translate from raw ADC value to + * to Vbus voltage in volts. + * + * @return float Vbus voltage + */ +float currsen_get_vbus_voltage() +{ + return VBUS_SCALE * ((float) m_adc_result.Vbus_raw); +} + +/** + * @brief gets the temperature. Translate from raw ADC value to + * to scaled raw ADC value to temperature. + * + * @return int32_t temperature in C + */ + +int32_t currsen_get_temperature() +{ + // From A.7.16 of RM0091 + int32_t temperature; + // Scale the raw ADC value to the VDD_CALIB value and center based on the + // temperature calibration points. + temperature = (((int32_t) m_adc_result.Spin_temperature_raw * VDD_APPLI / VDD_CALIB) - (int32_t)(*TEMP30_CAL_ADDR)); + // Scale by the difference between the two calibration points. + temperature = temperature * (int32_t)(110 - 30); + temperature = temperature / (int32_t)(*TEMP110_CAL_ADDR - *TEMP30_CAL_ADDR); + // Add the offset of 30 degrees Celsius from the base calibration point. + temperature = temperature + 30; + return temperature; +} + +void init_motor_current_adc_offset() +{ + // Read the motor current offset from flash. + // If not set, set to 0.0f. + volatile uint32_t* current_offset_ptr = (uint32_t*) CURRENT_OFFSET_ADDRESS; + if (*current_offset_ptr != CURRENT_OFFSET_MAGIC) { + m_motor_adc_offset = 0.0f; + } else { + // If the magic number matches, read the float value one after the magic number. + m_motor_adc_offset = *((float*) (current_offset_ptr + 1)); + } } \ No newline at end of file diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h index 0dc4ca3c..bcec03fc 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -1,28 +1,77 @@ /** * @file setup.h - * @author your name (you@domain.com) - * @brief + * @author Joe Spall + * @brief * @version 0.1 - * @date 2022-04-10 - * - * @copyright Copyright (c) 2022 - * + * + * @copyright Copyright (c) 2025 + * */ #pragma once +#include +#include + +static const float V_DDA_MV = 3000.0f; // mV +static const float V_DDA_V = 3.0f; // V +static const float V_ADC_SCALE_MV = (V_DDA_MV / 4095.0f); // mV +static const float V_ADC_SCALE_V = (V_DDA_V / 4095.0f); // V + +// From A.7.16 of RM0091 +#define TEMP110_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7C2)) +#define TEMP30_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7B8)) + +#define VDD_CALIB ((uint16_t) (3300)) +#define VDD_APPLI ((uint16_t) (V_DDA_MV)) + +// Motor Current scaling +// In v1.0 motor-controller board, schematic based off of AN5397. +// Rs = 2x 0.1 ohm in parallel -> 0.05 ohm +static const float MOTOR_OPAMP_RESISTOR_A = 140E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_B = 2.370E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_1 = 10E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_2 = 40.2E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_SENSE = 0.05f; // ohm +// V_dyn_range = VDD - (0.2 * 2) = 2.6V +// NOTE: 0.2V is the safety margin for the op-amp from STSPIN32 datasheet. +// SR = 10V/us op-amp slew rate +// Gmax = V_dyn_range / (Imax * Rs) = 2.6 / (10.0 * 0.05) = 5.2 +// NOTE: Using Imax * Rs instead of 2 * Imax * Rs since not measuring negative current. +// G_real = (Ra/(Ra + Rb)) * (1 + R2/R1) = (140k/(140k + 2.37k)) * (1 + 40.2k/10.0k) = 0.983 * 5.02 = 4.94 +// static const float MOTOR_OPAMP_GAIN_REAL = ((MOTOR_OPAMP_RESISTOR_A / (MOTOR_OPAMP_RESISTOR_A + MOTOR_OPAMP_RESISTOR_B)) * (1.0f + (MOTOR_OPAMP_RESISTOR_2 / MOTOR_OPAMP_RESISTOR_1))); +// BW = GBP/(1 + R2/R1) = 18MHz / (1 + 40.2k/10k) = 18MHz / 5.02 = 3.58MHz +// Closed Loop Gain = 1 + R2/R1 = 1 + 40.2k/10k = 5.02 > Closed Loop Gain Min of STSPIN = 4 +// T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us + +static const float MOTOR_OPAMP_GAIN_REAL = 5.5; + +// Op-amp is configured so 0A motor current = 0.2V +static const float V_MIN_OP_AMP = 0.25f; // V + +// VBUS voltage scaling +// 11.5k / 1k resistor divider -> 12.5 scaling +// 12.5 * V_ADC_SCALE = VBUS_SCALE +// V_ADC_SCALE_V = 3.0V / 4095.0f ADC resolution +// NOTE: Compute ahead of time to reduce floating point math. +// V_bus = V_adc_raw * VBUS_SCALE +static const float VBUS_SCALE = 0.0091575f; + +// TODO Move to software-communication +static const uint32_t CURRENT_OFFSET_ADDRESS = 0x08007C00; +static const uint32_t CURRENT_OFFSET_MAGIC = 0xAABBCCDD; + typedef enum { - CS_MODE_POLLING, - CS_MODE_DMA, - CS_MODE_TIMER_DMA + CS_MODE_PWM_DMA, + CS_MODE_SOFTWARE } CS_Mode_t; -typedef enum +typedef enum { - CS_OK = 0x00U, - CS_ERROR = 0x01U, - CS_BUSY = 0x02U, - CS_TIMEOUT = 0x03U + CS_OK = 0x00U, + CS_ERROR = 0x01U, + CS_BUSY = 0x02U, + CS_TIMEOUT = 0x03U } CS_Status_t; #define SET_BIT(REG, BIT) ((REG) |= (BIT)) @@ -82,31 +131,38 @@ typedef enum // TODO tune timing #define ADC_STP_TIMEOUT 5 //ms +#define ADC_SOFTWARE_CONVERSION_TIMEOUT 2 // ms +#define ADC_MOTOR_CURRENT_ZERO_SAMPLE_NUM 4 +#define UINT_12_BIT_MAX 4095 // 2^12 - 1, max value for 12 bit ADC -// this struct is used as a DMA target. +// this struct is used as a DMA target. // ADC->DR reads are two bytes, DMA will do half word transfers // rm0091 tells us the 16->32 bit port mapping packing scheme // which all us to derive that this struct should be packed -// hald words. Additionally, result must be at the end since a +// half words. Additionally, result must be at the end since a // ref to this struct will be passed into DMAR register for N // transfers -typedef struct +typedef struct __attribute__((__packed__)) ADC_Result { - uint16_t I_motor_filt; - uint16_t I_motor; - uint16_t T_spin; - uint16_t V_int; - CS_Status_t status; + uint16_t Motor_current_raw; + uint16_t Vbus_raw; + uint16_t Spin_temperature_raw; } ADC_Result_t; void currsen_enable_ht(); void currsen_read_dma(); -void currsen_read(ADC_Result_t *res); -CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uint32_t ch_sel, uint32_t sr_sel); -CS_Status_t currsen_adc_group_config(); -CS_Status_t currsen_adc_conf(); +CS_Status_t currsen_setup(uint8_t motor_adc_ch); CS_Status_t currsen_adc_cal(); CS_Status_t currsen_adc_en(); CS_Status_t currsen_adc_dis(); - - +CS_Status_t calculate_motor_zero_current_setpoint(); + +float currsen_get_shunt_voltage_raw(); +float currsen_get_shunt_voltage(); +float currsen_get_motor_current(); +float currsen_get_motor_current_with_offset(); +float currsen_get_motor_current_offset(); +bool currsen_calibrate_sense(); +float currsen_get_vbus_voltage(); +int32_t currsen_get_temperature(); +uint16_t currsen_get_motor_current_zero_adc_raw(); diff --git a/software-communication b/software-communication index a4ef89e4..9ce093cf 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit a4ef89e4aa9081f9c3b3e7eff8546d8f2c7644fb +Subproject commit 9ce093cf5920b20ac466da2b5f12ed84c6786bb1