From 6135e09316528f64b8f4f58cfae6c4121e0e20ad Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:00:51 -0400 Subject: [PATCH 01/22] add rustfmt check to ci --- .github/workflows/rustfmt.yml | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 .github/workflows/rustfmt.yml diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml new file mode 100644 index 00000000..4cca58de --- /dev/null +++ b/.github/workflows/rustfmt.yml @@ -0,0 +1,31 @@ + name: Rustfmt Check + + on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + + jobs: + format: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions-rust-lang/setup-rust-toolchain@v1 + with: + toolchain: nightly + components: rustfmt + - name: Common + run: cargo fmt --check --manifest-path common + - name: Library + run: cargo fmt --check --manifest-path lib-stm32 + - name: LIbrary Test Images + run: cargo fmt --check --manifest-path lib-stm32-test + - name: Credentials + run: cargo fmt --check --manifest-path credentials + - name: Power Board + run: cargo fmt --check --manifest-path power-board + - name: Kicker Board + run: cargo fmt --check --manifest-path kicker-board + - name: Control Board + run: cargo fmt --check --manifest-path control-board \ No newline at end of file From 827b152a94bf1815d0645cfb474ce13f0c659ed0 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:03:17 -0400 Subject: [PATCH 02/22] fix fmt check path --- .github/workflows/rustfmt.yml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml index 4cca58de..1be097c0 100644 --- a/.github/workflows/rustfmt.yml +++ b/.github/workflows/rustfmt.yml @@ -16,16 +16,16 @@ toolchain: nightly components: rustfmt - name: Common - run: cargo fmt --check --manifest-path common + run: cargo fmt --check --manifest-path common/Cargo.toml - name: Library - run: cargo fmt --check --manifest-path lib-stm32 + run: cargo fmt --check --manifest-path lib-stm32/Cargo.toml - name: LIbrary Test Images - run: cargo fmt --check --manifest-path lib-stm32-test + run: cargo fmt --check --manifest-path lib-stm32-test/Cargo.toml - name: Credentials - run: cargo fmt --check --manifest-path credentials + run: cargo fmt --check --manifest-path credentials/Cargo.toml - name: Power Board - run: cargo fmt --check --manifest-path power-board + run: cargo fmt --check --manifest-path power-board/Cargo.toml - name: Kicker Board - run: cargo fmt --check --manifest-path kicker-board + run: cargo fmt --check --manifest-path kicker-board/Cargo.toml - name: Control Board - run: cargo fmt --check --manifest-path control-board \ No newline at end of file + run: cargo fmt --check --manifest-path control-board/Cargo.toml \ No newline at end of file From cd2fd06bcde3be7f597b05326682034e2116a36e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:17:13 -0400 Subject: [PATCH 03/22] add style util scripts and make CI style check quiet --- .github/workflows/rustfmt.yml | 14 ++++----- util/rust-format-all.bash | 11 +++++++ util/rust-format-check.bash | 57 +++++++++++++++++++++++++++++++++++ 3 files changed, 75 insertions(+), 7 deletions(-) create mode 100755 util/rust-format-all.bash create mode 100755 util/rust-format-check.bash diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml index 1be097c0..e0b3165c 100644 --- a/.github/workflows/rustfmt.yml +++ b/.github/workflows/rustfmt.yml @@ -16,16 +16,16 @@ toolchain: nightly components: rustfmt - name: Common - run: cargo fmt --check --manifest-path common/Cargo.toml + run: cargo fmt --quiet --check --manifest-path common/Cargo.toml - name: Library - run: cargo fmt --check --manifest-path lib-stm32/Cargo.toml + run: cargo fmt --quiet --check --manifest-path lib-stm32/Cargo.toml - name: LIbrary Test Images - run: cargo fmt --check --manifest-path lib-stm32-test/Cargo.toml + run: cargo fmt --quiet --check --manifest-path lib-stm32-test/Cargo.toml - name: Credentials - run: cargo fmt --check --manifest-path credentials/Cargo.toml + run: cargo fmt --quiet --check --manifest-path credentials/Cargo.toml - name: Power Board - run: cargo fmt --check --manifest-path power-board/Cargo.toml + run: cargo fmt --quiet --check --manifest-path power-board/Cargo.toml - name: Kicker Board - run: cargo fmt --check --manifest-path kicker-board/Cargo.toml + run: cargo fmt --quiet --check --manifest-path kicker-board/Cargo.toml - name: Control Board - run: cargo fmt --check --manifest-path control-board/Cargo.toml \ No newline at end of file + run: cargo fmt --quiet --check --manifest-path control-board/Cargo.toml \ No newline at end of file diff --git a/util/rust-format-all.bash b/util/rust-format-all.bash new file mode 100755 index 00000000..87bc857e --- /dev/null +++ b/util/rust-format-all.bash @@ -0,0 +1,11 @@ +#!/bin/bash + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) + +cargo fmt --manifest-path $SCRIPT_DIR/../common/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../lib-stm32/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../lib-stm32-test/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../credentials/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../power-board/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../kicker-board/Cargo.toml +cargo fmt --manifest-path $SCRIPT_DIR/../control-board/Cargo.toml \ No newline at end of file diff --git a/util/rust-format-check.bash b/util/rust-format-check.bash new file mode 100755 index 00000000..87d1ddc7 --- /dev/null +++ b/util/rust-format-check.bash @@ -0,0 +1,57 @@ +#!/bin/bash + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +ANY_FORMAT_CHECK_FAILED=false + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../common/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../lib-stm32/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../lib-stm32-test/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../credentials/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../power-board/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../kicker-board/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../control-board/Cargo.toml +if [ $? -ne 0]; then + echo "A-Team Common failed style check!" + ANY_FORMAT_CHECK_FAILED=true +fi + +echo "" +echo "" + +if [ "$ANY_FORMAT_CHECK_FAILED" = true ]; then + echo "One or more file failed the style check!" +elif + echo "All files passed the style check." +fi + +echo "" \ No newline at end of file From 8cd9546dead44f69a25f31faddb815f1d1cd4e78 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:21:58 -0400 Subject: [PATCH 04/22] fix format check script --- util/rust-format-check.bash | 38 ++++++++++++++++++++----------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/util/rust-format-check.bash b/util/rust-format-check.bash index 87d1ddc7..1d63accd 100755 --- a/util/rust-format-check.bash +++ b/util/rust-format-check.bash @@ -4,44 +4,44 @@ SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) ANY_FORMAT_CHECK_FAILED=false cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../common/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team common failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../lib-stm32/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team library lib-stm32 failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../lib-stm32-test/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team library test lib-stm32-test failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../credentials/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team library credentials failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../power-board/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team firmware image power-board failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../kicker-board/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team firmware image kicker-board failed style check!" ANY_FORMAT_CHECK_FAILED=true fi cargo fmt --quiet --check --manifest-path $SCRIPT_DIR/../control-board/Cargo.toml -if [ $? -ne 0]; then - echo "A-Team Common failed style check!" +if [ $? -ne 0 ]; then + echo "A-Team firmware image control-board failed style check!" ANY_FORMAT_CHECK_FAILED=true fi @@ -49,9 +49,13 @@ echo "" echo "" if [ "$ANY_FORMAT_CHECK_FAILED" = true ]; then - echo "One or more file failed the style check!" -elif - echo "All files passed the style check." + echo "############" + echo "# FAILED #" + echo "############" +else + echo "############" + echo "# PASSED #" + echo "############" fi echo "" \ No newline at end of file From a96d9eb772dafeb61ebb0607e3e0b25ae506ef89 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:24:29 -0400 Subject: [PATCH 05/22] make firmware pass style check --- common/src/bin/radio-testing/main.rs | 2 +- control-board/src/bin/control/main.rs | 101 +- control-board/src/bin/hwtest-bringup/main.rs | 13 +- control-board/src/bin/hwtest-control/main.rs | 95 +- control-board/src/bin/hwtest-dotstar/main.rs | 24 +- control-board/src/bin/hwtest-drib/main.rs | 58 +- control-board/src/bin/hwtest-imu/main.rs | 87 +- control-board/src/bin/hwtest-io/main.rs | 8 +- .../src/bin/hwtest-kicker-coms/main.rs | 27 +- control-board/src/bin/hwtest-kicker/main.rs | 28 +- control-board/src/bin/hwtest-motor/main.rs | 80 +- control-board/src/bin/hwtest-piezo/main.rs | 38 +- control-board/src/bin/hwtest-radio/main.rs | 100 +- control-board/src/drivers/kicker.rs | 90 +- control-board/src/drivers/mod.rs | 2 +- control-board/src/drivers/radio_robot.rs | 152 ++- control-board/src/drivers/shell_indicator.rs | 8 +- control-board/src/image_hash.rs | 10 +- control-board/src/lib.rs | 148 +-- .../src/motion/constant_gain_kalman_filter.rs | 44 +- control-board/src/motion/mod.rs | 2 +- .../motion/params/body_vel_filter_params.rs | 32 +- .../src/motion/params/body_vel_pid_params.rs | 15 +- control-board/src/motion/params/mod.rs | 2 +- .../motion/params/robot_physical_params.rs | 2 +- control-board/src/motion/pid.rs | 23 +- control-board/src/motion/robot_controller.rs | 233 ++-- control-board/src/motion/robot_model.rs | 22 +- control-board/src/parameter_interface.rs | 5 +- control-board/src/pins.rs | 138 ++- control-board/src/robot_state.rs | 23 +- control-board/src/songs.rs | 154 ++- control-board/src/stspin_motor.rs | 105 +- control-board/src/tasks/audio_task.rs | 40 +- control-board/src/tasks/control_task.rs | 1036 ++++++++++------- control-board/src/tasks/dotstar_task.rs | 122 +- control-board/src/tasks/imu_task.rs | 240 ++-- control-board/src/tasks/kicker_task.rs | 211 +++- control-board/src/tasks/mod.rs | 4 +- control-board/src/tasks/power_task.rs | 153 ++- control-board/src/tasks/radio_task.rs | 469 ++++++-- control-board/src/tasks/user_io_task.rs | 223 +++- control-board/tests/basic-test.rs | 4 +- control-board/tests/drivetrain-test.rs | 14 +- control-board/tests/uart-queue.rs | 8 +- credentials/src/lib.rs | 4 +- credentials/src/public_credentials/mod.rs | 2 +- credentials/src/public_credentials/wifi.rs | 12 +- kicker-board/src/bin/hwtest-blinky/main.rs | 65 +- kicker-board/src/bin/hwtest-breakbeam/main.rs | 26 +- kicker-board/src/bin/hwtest-charge/main.rs | 50 +- kicker-board/src/bin/hwtest-coms/main.rs | 97 +- kicker-board/src/bin/hwtest-dribbler/main.rs | 43 +- kicker-board/src/bin/hwtest-flash/main.rs | 8 +- kicker-board/src/bin/hwtest-kick/main.rs | 58 +- kicker-board/src/bin/hwtest-kickstr/main.rs | 67 +- kicker-board/src/bin/kicker/main.rs | 176 ++- kicker-board/src/drivers/breakbeam.rs | 15 +- kicker-board/src/drivers/mod.rs | 111 +- kicker-board/src/image_hash.rs | 8 +- kicker-board/src/kick_manager.rs | 58 +- kicker-board/src/lib.rs | 20 +- kicker-board/src/pins.rs | 8 +- kicker-board/src/tasks/mod.rs | 15 +- .../src/bin/hwtest-adv-btn-async/main.rs | 7 +- .../src/bin/hwtest-adv-btn-poll/main.rs | 7 +- .../bin/hwtest-uart-queue-mixedbaud/main.rs | 84 +- .../bin/hwtest-uart-queue-mixedrate/main.rs | 63 +- lib-stm32/build.rs | 38 +- lib-stm32/src/anim/mod.rs | 189 ++- lib-stm32/src/audio/mod.rs | 6 +- lib-stm32/src/audio/note.rs | 4 +- lib-stm32/src/audio/songs.rs | 4 +- lib-stm32/src/audio/tone_player.rs | 20 +- lib-stm32/src/drivers/adc.rs | 15 +- lib-stm32/src/drivers/audio/buzzer.rs | 17 +- lib-stm32/src/drivers/audio/mod.rs | 2 +- lib-stm32/src/drivers/boot/mod.rs | 2 +- lib-stm32/src/drivers/boot/stm32_interface.rs | 455 ++++---- lib-stm32/src/drivers/flash/at25df041b.rs | 19 +- lib-stm32/src/drivers/flash/mod.rs | 3 +- lib-stm32/src/drivers/imu/bmi085.rs | 192 ++- lib-stm32/src/drivers/imu/bmi323.rs | 122 +- lib-stm32/src/drivers/imu/mod.rs | 1 - lib-stm32/src/drivers/led/apa102.rs | 86 +- lib-stm32/src/drivers/led/mod.rs | 4 +- lib-stm32/src/drivers/mod.rs | 2 +- lib-stm32/src/drivers/other/adc_helper.rs | 24 +- lib-stm32/src/drivers/other/mod.rs | 2 +- lib-stm32/src/drivers/radio/at_protocol.rs | 122 +- lib-stm32/src/drivers/radio/edm_protocol.rs | 3 +- lib-stm32/src/drivers/radio/mod.rs | 2 +- lib-stm32/src/drivers/radio/odin_w26x.rs | 155 +-- lib-stm32/src/drivers/switches/button.rs | 74 +- lib-stm32/src/drivers/switches/dip.rs | 28 +- lib-stm32/src/drivers/switches/mod.rs | 2 +- .../src/drivers/switches/rotary_encoder.rs | 16 +- lib-stm32/src/filter/mod.rs | 24 +- lib-stm32/src/lib.rs | 9 +- lib-stm32/src/math/lerp.rs | 35 +- lib-stm32/src/math/linear_map.rs | 20 +- lib-stm32/src/math/mod.rs | 14 +- lib-stm32/src/math/range.rs | 7 +- lib-stm32/src/power/battery.rs | 51 +- lib-stm32/src/power/mod.rs | 22 +- lib-stm32/src/power/model/lipo_model.rs | 97 +- lib-stm32/src/power/model/mod.rs | 2 +- lib-stm32/src/queue.rs | 38 +- lib-stm32/src/time/mod.rs | 8 +- lib-stm32/src/uart/mod.rs | 2 +- lib-stm32/src/uart/queue.rs | 358 +++--- lib-stm32/src/units.rs | 3 +- lib-stm32/tests/linear-map.rs | 5 +- power-board/src/bin/hwtest-bringup/main.rs | 64 +- power-board/src/bin/power/main.rs | 64 +- power-board/src/bin/song-test/main.rs | 68 +- power-board/src/config.rs | 50 +- power-board/src/lib.rs | 4 +- power-board/src/pins.rs | 57 +- power-board/src/power_state.rs | 35 +- power-board/src/songs.rs | 209 +++- power-board/src/tasks/audio_task.rs | 75 +- power-board/src/tasks/coms_task.rs | 95 +- power-board/src/tasks/io_task.rs | 1 + power-board/src/tasks/led_task.rs | 10 +- power-board/src/tasks/mod.rs | 2 +- power-board/src/tasks/power_task.rs | 269 +++-- 127 files changed, 5625 insertions(+), 3081 deletions(-) diff --git a/common/src/bin/radio-testing/main.rs b/common/src/bin/radio-testing/main.rs index 43e16d0a..b253b3d7 100644 --- a/common/src/bin/radio-testing/main.rs +++ b/common/src/bin/radio-testing/main.rs @@ -77,7 +77,7 @@ fn main() -> std::io::Result<()> { vel_z_angular: 0., kick_vel: 0., dribbler_speed: 0., - kick_request: KickRequest::KR_ARM + kick_request: KickRequest::KR_ARM, }, }, }; diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 85c577c8..9dc89cfc 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -2,15 +2,20 @@ #![no_main] use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - interrupt, pac::Interrupt, wdg::IndependentWatchdog -}; +use embassy_stm32::{interrupt, pac::Interrupt, wdg::IndependentWatchdog}; use embassy_sync::pubsub::PubSubChannel; -use defmt_rtt as _; +use defmt_rtt as _; use ateam_control_board::{ - create_audio_task, create_control_task, create_dotstar_task, create_imu_task, create_io_task, create_kicker_task, create_power_task, create_radio_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; + create_audio_task, create_control_task, create_dotstar_task, create_imu_task, create_io_task, + create_kicker_task, create_power_task, create_radio_task, get_system_config, + pins::{ + AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, + PowerTelemetryPubSub, TelemetryPubSub, + }, + robot_state::SharedRobotState, +}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -31,7 +36,8 @@ use static_cell::ConstStaticCell; // cortex_m::peripheral::SCB::sys_reset(); // } -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -70,12 +76,18 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup task pools // //////////////////////// - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CORDIC, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CORDIC, + embassy_stm32::interrupt::Priority::P6, + ); let radio_uart_queue_spawner = RADIO_UART_QUEUE_EXECUTOR.start(Interrupt::CORDIC); // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P7, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -117,46 +129,63 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); - create_dotstar_task!(main_spawner, - led_command_subscriber, - p); + create_dotstar_task!(main_spawner, led_command_subscriber, p); - create_audio_task!(main_spawner, - robot_state, - p); + create_audio_task!(main_spawner, robot_state, p); - create_radio_task!(main_spawner, radio_uart_queue_spawner, radio_uart_queue_spawner, + create_radio_task!( + main_spawner, + radio_uart_queue_spawner, + radio_uart_queue_spawner, robot_state, - radio_command_publisher, radio_telemetry_subscriber, radio_led_cmd_publisher, + radio_command_publisher, + radio_telemetry_subscriber, + radio_led_cmd_publisher, wifi_credentials, - p); - - create_power_task!(main_spawner, uart_queue_spawner, - robot_state, power_board_telemetry_publisher, power_led_cmd_publisher, - p); + p + ); - create_imu_task!(main_spawner, + create_power_task!( + main_spawner, + uart_queue_spawner, robot_state, - imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher, - p); + power_board_telemetry_publisher, + power_led_cmd_publisher, + p + ); - create_control_task!(main_spawner, uart_queue_spawner, - robot_state, - control_command_subscriber, control_telemetry_publisher, - control_task_power_telemetry_subscriber, control_task_kicker_telemetry_subscriber, - control_gyro_data_subscriber, control_accel_data_subscriber, - p); + create_imu_task!( + main_spawner, + robot_state, + imu_gyro_data_publisher, + imu_accel_data_publisher, + imu_led_cmd_publisher, + p + ); + + create_control_task!( + main_spawner, + uart_queue_spawner, + robot_state, + control_command_subscriber, + control_telemetry_publisher, + control_task_power_telemetry_subscriber, + control_task_kicker_telemetry_subscriber, + control_gyro_data_subscriber, + control_accel_data_subscriber, + p + ); create_kicker_task!( - main_spawner, uart_queue_spawner, + main_spawner, + uart_queue_spawner, robot_state, kicker_command_subscriber, kicker_board_telemetry_publisher, - p); + p + ); let mut iwdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); iwdg.unleash(); @@ -165,4 +194,4 @@ async fn main(main_spawner: embassy_executor::Spawner) { Timer::after_millis(100).await; iwdg.pet(); } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-bringup/main.rs b/control-board/src/bin/hwtest-bringup/main.rs index f728c9c7..97620347 100644 --- a/control-board/src/bin/hwtest-bringup/main.rs +++ b/control-board/src/bin/hwtest-bringup/main.rs @@ -2,10 +2,9 @@ #![no_main] #![feature(generic_const_exprs)] - +use ateam_control_board::get_system_config; use ateam_lib_stm32::drivers::led::apa102::Apa102; use embassy_stm32::gpio::{Level, Output, Speed}; -use ateam_control_board::get_system_config; const DOTSTAR_BUF_SIZE: usize = 8 + (11 * 4); @@ -29,10 +28,12 @@ async fn main(_main_spawner: embassy_executor::Spawner) { let mut led2 = Output::new(p.PG5, Level::Low, Speed::Low); let mut led1 = Output::new(p.PG6, Level::Low, Speed::Low); - // Get the pins from the schematic - let dotstar_spi_buf: &'static mut [u8; DOTSTAR_BUF_SIZE] = unsafe { &mut DOTSTAR_SPI_BUFFER_CELL }; + // Get the pins from the schematic + let dotstar_spi_buf: &'static mut [u8; DOTSTAR_BUF_SIZE] = + unsafe { &mut DOTSTAR_SPI_BUFFER_CELL }; // Dotstar SPI, SCK, MOSI, and TX_DMA - let mut dotstars = Apa102::<11>::new_from_pins(p.SPI6, p.PB3, p.PB5, p.BDMA_CH0, dotstar_spi_buf.into()); + let mut dotstars = + Apa102::<11>::new_from_pins(p.SPI6, p.PB3, p.PB5, p.BDMA_CH0, dotstar_spi_buf.into()); dotstars.set_drv_str_all(32); loop { @@ -50,4 +51,4 @@ async fn main(_main_spawner: embassy_executor::Spawner) { dotstars.update().await; Timer::after_millis(1000).await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-control/main.rs b/control-board/src/bin/hwtest-control/main.rs index eabf7a69..6753c007 100644 --- a/control-board/src/bin/hwtest-control/main.rs +++ b/control-board/src/bin/hwtest-control/main.rs @@ -1,17 +1,22 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings::KickRequest, bindings::BasicControl, radio::DataPacket}; +use ateam_common_packets::{bindings::BasicControl, bindings::KickRequest, radio::DataPacket}; use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - interrupt, pac::Interrupt -}; +use embassy_stm32::{interrupt, pac::Interrupt}; use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; use ateam_control_board::{ - create_audio_task, create_control_task, create_dotstar_task, create_imu_task, create_io_task, create_kicker_task, create_radio_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; + create_audio_task, create_control_task, create_dotstar_task, create_imu_task, create_io_task, + create_kicker_task, create_radio_task, get_system_config, + pins::{ + AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, + PowerTelemetryPubSub, TelemetryPubSub, + }, + robot_state::SharedRobotState, +}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -32,7 +37,8 @@ fn panic(info: &core::panic::PanicInfo) -> ! { cortex_m::peripheral::SCB::sys_reset(); } -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static RADIO_DUMMY_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); @@ -72,12 +78,18 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup task pools // //////////////////////// - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CORDIC, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CORDIC, + embassy_stm32::interrupt::Priority::P6, + ); let radio_uart_queue_spawner = RADIO_UART_QUEUE_EXECUTOR.start(Interrupt::CORDIC); // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P7, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -116,42 +128,55 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); - create_dotstar_task!(main_spawner, - led_command_subscriber, - p); + create_dotstar_task!(main_spawner, led_command_subscriber, p); - create_audio_task!(main_spawner, - robot_state, - p); + create_audio_task!(main_spawner, robot_state, p); - create_radio_task!(main_spawner, radio_uart_queue_spawner, uart_queue_spawner, - // create_radio_task!(main_spawner, uart_queue_spawner, + create_radio_task!( + main_spawner, + radio_uart_queue_spawner, + uart_queue_spawner, + // create_radio_task!(main_spawner, uart_queue_spawner, robot_state, - radio_dummy_command_publisher, radio_telemetry_subscriber, radio_led_cmd_publisher, + radio_dummy_command_publisher, + radio_telemetry_subscriber, + radio_led_cmd_publisher, wifi_credentials, - p); + p + ); - create_imu_task!(main_spawner, + create_imu_task!( + main_spawner, robot_state, - imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher, - p); - - create_control_task!(main_spawner, uart_queue_spawner, + imu_gyro_data_publisher, + imu_accel_data_publisher, + imu_led_cmd_publisher, + p + ); + + create_control_task!( + main_spawner, + uart_queue_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, - control_task_power_telemetry_subscriber, control_task_kicker_telemetry_subscriber, - control_gyro_data_subscriber, control_accel_data_subscriber, - p); + control_command_subscriber, + control_telemetry_publisher, + control_task_power_telemetry_subscriber, + control_task_kicker_telemetry_subscriber, + control_gyro_data_subscriber, + control_accel_data_subscriber, + p + ); create_kicker_task!( - main_spawner, uart_queue_spawner, + main_spawner, + uart_queue_spawner, robot_state, - kicker_command_subscriber, kicker_board_telemetry_publisher, - p); + kicker_command_subscriber, + kicker_board_telemetry_publisher, + p + ); loop { Timer::after_millis(10).await; @@ -166,7 +191,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { vel_z_angular: 0.0, kick_vel: 0.0, dribbler_speed: -0.1, - kick_request: KickRequest::KR_ARM + kick_request: KickRequest::KR_ARM, })); } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-dotstar/main.rs b/control-board/src/bin/hwtest-dotstar/main.rs index d9937182..2ccaf0cb 100644 --- a/control-board/src/bin/hwtest-dotstar/main.rs +++ b/control-board/src/bin/hwtest-dotstar/main.rs @@ -4,10 +4,14 @@ use embassy_executor::InterruptExecutor; use embassy_stm32::interrupt; -use defmt_rtt as _; - -use ateam_control_board::{create_dotstar_task, create_io_task, get_system_config, pins::LedCommandPubSub, robot_state::SharedRobotState, tasks::dotstar_task::{ControlBoardLedCommand, ControlGeneralLedCommand}}; +use defmt_rtt as _; +use ateam_control_board::{ + create_dotstar_task, create_io_task, get_system_config, + pins::LedCommandPubSub, + robot_state::SharedRobotState, + tasks::dotstar_task::{ControlBoardLedCommand, ControlGeneralLedCommand}, +}; use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; @@ -15,7 +19,8 @@ use embassy_time::Timer; use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static LED_COMMAND_CHANNEL: LedCommandPubSub = PubSubChannel::new(); @@ -44,7 +49,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { ////////////////////////////////////// // setup inter-task coms channels // ////////////////////////////////////// - + let led_command_subscriber = LED_COMMAND_CHANNEL.subscriber().unwrap(); let led_command_publisher = LED_COMMAND_CHANNEL.publisher().unwrap(); @@ -58,10 +63,13 @@ async fn main(main_spawner: embassy_executor::Spawner) { // create_audio_task!(main_spawner, robot_state, p); - led_command_publisher.publish(ControlBoardLedCommand::General(ControlGeneralLedCommand::ShutdownRequested)).await; - + led_command_publisher + .publish(ControlBoardLedCommand::General( + ControlGeneralLedCommand::ShutdownRequested, + )) + .await; loop { Timer::after_millis(1000).await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-drib/main.rs b/control-board/src/bin/hwtest-drib/main.rs index 83e3a106..d1b7643d 100644 --- a/control-board/src/bin/hwtest-drib/main.rs +++ b/control-board/src/bin/hwtest-drib/main.rs @@ -1,23 +1,26 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings::KickRequest, bindings::BasicControl, radio::DataPacket}; +use ateam_common_packets::{bindings::BasicControl, bindings::KickRequest, radio::DataPacket}; use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - interrupt, pac::Interrupt -}; +use embassy_stm32::{interrupt, pac::Interrupt}; use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_io_task, create_kicker_task, get_system_config, pins::{CommandsPubSub, KickerTelemetryPubSub}, robot_state::SharedRobotState}; +use ateam_control_board::{ + create_io_task, create_kicker_task, get_system_config, + pins::{CommandsPubSub, KickerTelemetryPubSub}, + robot_state::SharedRobotState, +}; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static KICKER_DATA_CHANNEL: KickerTelemetryPubSub = PubSubChannel::new(); @@ -53,7 +56,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P7, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -68,29 +74,31 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); create_kicker_task!( - main_spawner, uart_queue_spawner, + main_spawner, + uart_queue_spawner, robot_state, - kicker_command_subscriber, kicker_telemetry_publisher, - p); - + kicker_command_subscriber, + kicker_telemetry_publisher, + p + ); loop { Timer::after_millis(100).await; - test_command_publisher.publish(DataPacket::BasicControl(BasicControl { - _bitfield_1: Default::default(), - _bitfield_align_1: Default::default(), - vel_x_linear: 0.0, - vel_y_linear: 0.0, - vel_z_angular: 0.0, - kick_vel: 0.0, - dribbler_speed: 50.0, - kick_request: KickRequest::KR_DISABLE, - })).await; + test_command_publisher + .publish(DataPacket::BasicControl(BasicControl { + _bitfield_1: Default::default(), + _bitfield_align_1: Default::default(), + vel_x_linear: 0.0, + vel_y_linear: 0.0, + vel_z_angular: 0.0, + kick_vel: 0.0, + dribbler_speed: 50.0, + kick_request: KickRequest::KR_DISABLE, + })) + .await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index 8bc8a568..380bfaa0 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -6,16 +6,21 @@ use embassy_futures::select::{self, Either3}; use embassy_stm32::interrupt; use embassy_sync::pubsub::{PubSubChannel, WaitResult}; -use defmt_rtt as _; +use defmt_rtt as _; -use ateam_control_board::{create_dotstar_task, create_imu_task, create_io_task, get_system_config, pins::{AccelDataPubSub, GyroDataPubSub, LedCommandPubSub}, robot_state::SharedRobotState}; +use ateam_control_board::{ + create_dotstar_task, create_imu_task, create_io_task, get_system_config, + pins::{AccelDataPubSub, GyroDataPubSub, LedCommandPubSub}, + robot_state::SharedRobotState, +}; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); @@ -43,11 +48,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup task pools // //////////////////////// - ////////////////////////////////////// // setup inter-task coms channels // ////////////////////////////////////// - + let led_command_subscriber = LED_COMMAND_PUBSUB.subscriber().unwrap(); let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); @@ -55,54 +59,63 @@ async fn main(main_spawner: embassy_executor::Spawner) { let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); let mut imu_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); let imu_led_cmd_publisher = LED_COMMAND_PUBSUB.publisher().unwrap(); - /////////////////// // start tasks // /////////////////// - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); - create_dotstar_task!(main_spawner, - led_command_subscriber, - p); + create_dotstar_task!(main_spawner, led_command_subscriber, p); // create_audio_task!(main_spawner, robot_state, p); - create_imu_task!(main_spawner, + create_imu_task!( + main_spawner, robot_state, - imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher, - p); + imu_gyro_data_publisher, + imu_accel_data_publisher, + imu_led_cmd_publisher, + p + ); loop { - match select::select3(imu_gyro_data_subscriber.next_message(), imu_accel_data_subscriber.next_message(), Timer::after_millis(1000)).await { - Either3::First(gyro_data) => { - match gyro_data { - WaitResult::Lagged(amnt) => { - defmt::warn!("publishing gyro data lagged by {}", amnt); - } - WaitResult::Message(msg) => { - defmt::info!("got gyro data (x: {}, y: {}, z: {})", msg[1], msg[1], msg[2]); - - } + match select::select3( + imu_gyro_data_subscriber.next_message(), + imu_accel_data_subscriber.next_message(), + Timer::after_millis(1000), + ) + .await + { + Either3::First(gyro_data) => match gyro_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("publishing gyro data lagged by {}", amnt); } - } - Either3::Second(accel_data) => { - match accel_data { - WaitResult::Lagged(amnt) => { - defmt::warn!("publishing accel data lagged by {}", amnt); - } - WaitResult::Message(msg) => { - defmt::info!("got accel data (x: {}, y: {}, z: {})", msg[0], msg[1], msg[2]); - - } + WaitResult::Message(msg) => { + defmt::info!( + "got gyro data (x: {}, y: {}, z: {})", + msg[1], + msg[1], + msg[2] + ); } - } + }, + Either3::Second(accel_data) => match accel_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("publishing accel data lagged by {}", amnt); + } + WaitResult::Message(msg) => { + defmt::info!( + "got accel data (x: {}, y: {}, z: {})", + msg[0], + msg[1], + msg[2] + ); + } + }, Either3::Third(_) => { defmt::warn!("receiving timed out."); } } } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-io/main.rs b/control-board/src/bin/hwtest-io/main.rs index 287f74ef..3c211366 100644 --- a/control-board/src/bin/hwtest-io/main.rs +++ b/control-board/src/bin/hwtest-io/main.rs @@ -4,17 +4,17 @@ use embassy_executor::InterruptExecutor; use embassy_stm32::interrupt; -use defmt_rtt as _; +use defmt_rtt as _; use ateam_control_board::{create_io_task, get_system_config, robot_state::SharedRobotState}; - use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -53,4 +53,4 @@ async fn main(main_spawner: embassy_executor::Spawner) { loop { Timer::after_millis(1000).await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index 58a6de83..9cd06f65 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -4,17 +4,15 @@ #![feature(generic_const_exprs)] use ateam_control_board::{ - drivers::kicker::Kicker, get_system_config, include_kicker_bin, - DEBUG_KICKER_UART_QUEUES + drivers::kicker::Kicker, get_system_config, include_kicker_bin, DEBUG_KICKER_UART_QUEUES, }; use ateam_lib_stm32::{ drivers::boot::stm32_interface::{self, Stm32Interface}, - idle_buffered_uart_spawn_tasks, static_idle_buffered_uart}; + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, +}; use defmt::info; use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - gpio::Pull, interrupt, pac::Interrupt, usart::Uart -}; +use embassy_stm32::{gpio::Pull, interrupt, pac::Interrupt, usart::Uart}; use embassy_time::{Duration, Ticker}; use panic_probe as _; @@ -27,7 +25,6 @@ const RX_BUF_DEPTH: usize = 20; static_idle_buffered_uart!(KICKER, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_KICKER_UART_QUEUES, #[link_section = ".axisram.buffers"]); - static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[allow(non_snake_case)] @@ -43,7 +40,10 @@ async fn main(_spawner: embassy_executor::Spawner) { defmt::info!("Kicker system init"); - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P5, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); // loop { @@ -58,7 +58,8 @@ async fn main(_spawner: embassy_executor::Spawner) { p.DMA2_CH4, p.DMA2_CH5, stm32_interface::get_bootloader_uart_config(), - ).unwrap(); + ) + .unwrap(); defmt::info!("init uart"); @@ -74,7 +75,7 @@ async fn main(_spawner: embassy_executor::Spawner) { p.PA8, p.PA9, Pull::Up, - true + true, ); info!("flashing kicker..."); @@ -98,7 +99,11 @@ async fn main(_spawner: embassy_executor::Spawner) { kicker.process_telemetry(); // TODO print some telemetry or something - defmt::info!("high voltage: {}, battery voltage: {}", kicker.hv_rail_voltage(), kicker.battery_voltage()); + defmt::info!( + "high voltage: {}, battery voltage: {}", + kicker.hv_rail_voltage(), + kicker.battery_voltage() + ); kicker.send_command(); diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index 4403ab4e..4d06e9bf 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -4,19 +4,16 @@ #![feature(sync_unsafe_cell)] #![feature(generic_const_exprs)] - use ateam_control_board::{ - drivers::kicker::Kicker, get_system_config, include_kicker_bin, - DEBUG_KICKER_UART_QUEUES, + drivers::kicker::Kicker, get_system_config, include_kicker_bin, DEBUG_KICKER_UART_QUEUES, }; use ateam_lib_stm32::{ drivers::boot::stm32_interface::{self, Stm32Interface}, - idle_buffered_uart_spawn_tasks, static_idle_buffered_uart}; + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, +}; use defmt::info; use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - gpio::Pull, interrupt, pac::Interrupt, usart::Uart -}; +use embassy_stm32::{gpio::Pull, interrupt, pac::Interrupt, usart::Uart}; use embassy_time::{Duration, Ticker}; use panic_probe as _; @@ -29,7 +26,6 @@ const RX_BUF_DEPTH: usize = 20; static_idle_buffered_uart!(KICKER, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_KICKER_UART_QUEUES, #[link_section = ".axisram.buffers"]); - static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[allow(non_snake_case)] @@ -45,7 +41,10 @@ async fn main(_spawner: embassy_executor::Spawner) { defmt::info!("Kicker system init"); - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P5, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); let kicker_usart = Uart::new( @@ -56,7 +55,8 @@ async fn main(_spawner: embassy_executor::Spawner) { p.DMA2_CH2, p.DMA2_CH3, stm32_interface::get_bootloader_uart_config(), - ).unwrap(); + ) + .unwrap(); defmt::info!("init uart"); @@ -72,7 +72,7 @@ async fn main(_spawner: embassy_executor::Spawner) { p.PG2, p.PG3, Pull::Up, - true + true, ); info!("flashing kicker..."); @@ -96,7 +96,11 @@ async fn main(_spawner: embassy_executor::Spawner) { kicker.process_telemetry(); // TODO print some telemetry or something - defmt::info!("high voltage: {}, battery voltage: {}", kicker.hv_rail_voltage(), kicker.battery_voltage()); + defmt::info!( + "high voltage: {}, battery voltage: {}", + kicker.hv_rail_voltage(), + kicker.battery_voltage() + ); kicker.send_command(); diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 545db57c..9655cb6c 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -1,23 +1,29 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings::KickRequest, bindings::BasicControl, radio::DataPacket}; +use ateam_common_packets::{bindings::BasicControl, bindings::KickRequest, radio::DataPacket}; use embassy_executor::InterruptExecutor; -use embassy_stm32::{ - interrupt, pac::Interrupt -}; +use embassy_stm32::{interrupt, pac::Interrupt}; use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_control_task, create_imu_task, create_io_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; +use ateam_control_board::{ + create_control_task, create_imu_task, create_io_task, get_system_config, + pins::{ + AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, + PowerTelemetryPubSub, TelemetryPubSub, + }, + robot_state::SharedRobotState, +}; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -58,7 +64,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P7, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -90,35 +99,44 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); - create_imu_task!(main_spawner, + create_imu_task!( + main_spawner, robot_state, - imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher, - p); - - create_control_task!(main_spawner, uart_queue_spawner, + imu_gyro_data_publisher, + imu_accel_data_publisher, + imu_led_cmd_publisher, + p + ); + + create_control_task!( + main_spawner, + uart_queue_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, - control_task_power_telemetry_subscriber, control_task_kicker_telemetry_subscriber, - control_gyro_data_subscriber, control_accel_data_subscriber, - p); - + control_command_subscriber, + control_telemetry_publisher, + control_task_power_telemetry_subscriber, + control_task_kicker_telemetry_subscriber, + control_gyro_data_subscriber, + control_accel_data_subscriber, + p + ); loop { Timer::after_millis(100).await; - test_command_publisher.publish(DataPacket::BasicControl(BasicControl { - _bitfield_1: Default::default(), - _bitfield_align_1: Default::default(), - vel_x_linear: 1.0, - vel_y_linear: 0.0, - vel_z_angular: 0.0, - kick_vel: 0.0, - dribbler_speed: 10.0, - kick_request: KickRequest::KR_DISABLE, - })).await; + test_command_publisher + .publish(DataPacket::BasicControl(BasicControl { + _bitfield_1: Default::default(), + _bitfield_align_1: Default::default(), + vel_x_linear: 1.0, + vel_y_linear: 0.0, + vel_z_angular: 0.0, + kick_vel: 0.0, + dribbler_speed: 10.0, + kick_request: KickRequest::KR_DISABLE, + })) + .await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-piezo/main.rs b/control-board/src/bin/hwtest-piezo/main.rs index 11f69372..c2fe2402 100644 --- a/control-board/src/bin/hwtest-piezo/main.rs +++ b/control-board/src/bin/hwtest-piezo/main.rs @@ -4,20 +4,28 @@ use ateam_lib_stm32::{audio::tone_player::TonePlayer, drivers::audio::buzzer::Buzzer}; use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::OutputType, interrupt, time::khz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} + gpio::OutputType, + interrupt, + time::khz, + timer::{ + simple_pwm::{PwmPin, SimplePwm}, + Channel, + }, }; -use defmt_rtt as _; - -use ateam_control_board::{create_io_task, get_system_config, robot_state::SharedRobotState, songs::TEST_SONG}; +use defmt_rtt as _; +use ateam_control_board::{ + create_io_task, get_system_config, robot_state::SharedRobotState, songs::TEST_SONG, +}; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -48,15 +56,19 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - - - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); - let pwm = SimplePwm::new(p.TIM15, None, Some(ch2), None, None, khz(2), Default::default()); - + let pwm = SimplePwm::new( + p.TIM15, + None, + Some(ch2), + None, + None, + khz(2), + Default::default(), + ); + let audio_driver = Buzzer::new(pwm, Channel::Ch2); let mut tone_player = TonePlayer::new(audio_driver); @@ -69,4 +81,4 @@ async fn main(main_spawner: embassy_executor::Spawner) { loop { Timer::after_millis(10).await; } -} \ No newline at end of file +} diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 7ae95794..f2314db9 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -1,18 +1,22 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings::ParameterCommandCode, radio::{DataPacket, TelemetryPacket}}; +use ateam_common_packets::{ + bindings::ParameterCommandCode, + radio::{DataPacket, TelemetryPacket}, +}; use embassy_executor::InterruptExecutor; use embassy_futures::select::{self, Either}; -use embassy_stm32::{ - interrupt, pac::Interrupt -}; +use embassy_stm32::{interrupt, pac::Interrupt}; use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{create_dotstar_task, create_io_task, create_radio_task, get_system_config, pins::{CommandsPubSub, LedCommandPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; - +use ateam_control_board::{ + create_dotstar_task, create_io_task, create_radio_task, get_system_config, + pins::{CommandsPubSub, LedCommandPubSub, TelemetryPubSub}, + robot_state::SharedRobotState, +}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -33,7 +37,8 @@ fn panic(info: &core::panic::PanicInfo) -> ! { cortex_m::peripheral::SCB::sys_reset(); } -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static ROBOT_STATE: ConstStaticCell = + ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -64,7 +69,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CEC, + embassy_stm32::interrupt::Priority::P5, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -82,66 +90,68 @@ async fn main(main_spawner: embassy_executor::Spawner) { let led_command_subscriber = LED_COMMAND_PUBSUB.subscriber().unwrap(); - /////////////////// // start tasks // /////////////////// defmt::info!("starting tasks"); - create_io_task!(main_spawner, - robot_state, - p); + create_io_task!(main_spawner, robot_state, p); - create_dotstar_task!(main_spawner, - led_command_subscriber, - p); + create_dotstar_task!(main_spawner, led_command_subscriber, p); - create_radio_task!(main_spawner, uart_queue_spawner, uart_queue_spawner, + create_radio_task!( + main_spawner, + uart_queue_spawner, + uart_queue_spawner, robot_state, - radio_command_publisher, radio_telemetry_subscriber, radio_led_cmd_publisher, + radio_command_publisher, + radio_telemetry_subscriber, + radio_led_cmd_publisher, wifi_credentials, - p); + p + ); defmt::info!("radio task started"); loop { - match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { - Either::First(gyro_data) => { - match gyro_data { - WaitResult::Lagged(amnt) => { - defmt::warn!("receiving control packets lagged by {}", amnt); - } - WaitResult::Message(msg) => { - match msg { - DataPacket::BasicControl(_bc) => { - defmt::info!("got command packet"); + match select::select( + control_command_subscriber.next_message(), + Timer::after_millis(1000), + ) + .await + { + Either::First(gyro_data) => match gyro_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("receiving control packets lagged by {}", amnt); + } + WaitResult::Message(msg) => match msg { + DataPacket::BasicControl(_bc) => { + defmt::info!("got command packet"); - let basic_telem = Default::default(); + let basic_telem = Default::default(); - let pkt_wrapped = TelemetryPacket::Basic(basic_telem); - control_telemetry_publisher.publish(pkt_wrapped).await; + let pkt_wrapped = TelemetryPacket::Basic(basic_telem); + control_telemetry_publisher.publish(pkt_wrapped).await; - defmt::info!("send basic telem resp"); - } - DataPacket::ParameterCommand(pc) => { - defmt::info!("got parameter packet"); + defmt::info!("send basic telem resp"); + } + DataPacket::ParameterCommand(pc) => { + defmt::info!("got parameter packet"); - let mut param_resp = pc; - param_resp.command_code = ParameterCommandCode::PCC_ACK; + let mut param_resp = pc; + param_resp.command_code = ParameterCommandCode::PCC_ACK; - let wrapped_pkt = TelemetryPacket::ParameterCommandResponse(param_resp); - control_telemetry_publisher.publish(wrapped_pkt).await; + let wrapped_pkt = TelemetryPacket::ParameterCommandResponse(param_resp); + control_telemetry_publisher.publish(wrapped_pkt).await; - defmt::info!("sent param resp packet"); - } - } + defmt::info!("sent param resp packet"); } - } - } + }, + }, Either::Second(_) => { // defmt::warn!("packet processing timed out."); } } } -} \ No newline at end of file +} diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index b4d224d1..d73ea8c4 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -1,5 +1,11 @@ -use ateam_lib_stm32::{drivers::boot::stm32_interface::Stm32Interface, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; -use embassy_stm32::{gpio::{Pin, Pull}, usart::Parity}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface::Stm32Interface, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; +use embassy_stm32::{ + gpio::{Pin, Pull}, + usart::Parity, +}; use embassy_time::{with_timeout, Duration, Timer}; use ateam_common_packets::bindings::{KickerControl, KickerTelemetry}; @@ -13,14 +19,8 @@ pub struct Kicker< const DEPTH_RX: usize, const DEPTH_TX: usize, > { - stm32_uart_interface: Stm32Interface< - 'a, - LEN_RX, - LEN_TX, - DEPTH_RX, - DEPTH_TX, - DEBUG_KICKER_UART_QUEUES, - >, + stm32_uart_interface: + Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_KICKER_UART_QUEUES>, firmware_image: &'a [u8], command_state: KickerControl, @@ -59,14 +59,23 @@ impl< } } - pub fn new_from_pins(uart: &'a IdleBufferedUart, + pub fn new_from_pins( + uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, boot0_pin: impl Pin, reset_pin: impl Pin, - firmware_image: &'a [u8]) -> Self { - - let stm32_interface = Stm32Interface::new_from_pins(uart, read_queue, write_queue, boot0_pin, reset_pin, Pull::None, true); + firmware_image: &'a [u8], + ) -> Self { + let stm32_interface = Stm32Interface::new_from_pins( + uart, + read_queue, + write_queue, + boot0_pin, + reset_pin, + Pull::None, + true, + ); Self::new(stm32_interface, firmware_image) } @@ -77,7 +86,11 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("kicker interface - invalid packet of len {:?} data: {:?}", buf.len(), buf); + defmt::warn!( + "kicker interface - invalid packet of len {:?} data: {:?}", + buf.len(), + buf + ); continue; } @@ -93,7 +106,7 @@ impl< } } - return received_packet; + return received_packet; } pub fn send_command(&mut self) { @@ -113,7 +126,8 @@ impl< pub fn set_telemetry_enabled(&mut self, telemetry_enabled: bool) { self.telemetry_enabled = telemetry_enabled; - self.command_state.set_telemetry_enabled(telemetry_enabled as u32); + self.command_state + .set_telemetry_enabled(telemetry_enabled as u32); } pub fn request_kick(&mut self, request: u32) { @@ -208,8 +222,11 @@ impl< if received_telemetry { let current_img_hash = self.telemetry_state.kicker_image_hash; defmt::debug!("Kicker Interface - Received telemetry"); - defmt::trace!("Kicker Interface - Current device image hash {:x}", current_img_hash); - return current_img_hash + defmt::trace!( + "Kicker Interface - Current device image hash {:x}", + current_img_hash + ); + return current_img_hash; }; Timer::after(Duration::from_millis(5)).await; @@ -218,10 +235,15 @@ impl< pub async fn check_device_has_latest_default_image(&mut self) -> Result { let latest_img_hash = self.get_latest_default_img_hash(); - defmt::debug!("Kicker Interface - Latest default image hash - {:x}", latest_img_hash); + defmt::debug!( + "Kicker Interface - Latest default image hash - {:x}", + latest_img_hash + ); defmt::trace!("Kicker Interface - Update UART config 2 MHz"); - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; let res; @@ -233,14 +255,16 @@ impl< defmt::trace!("Kicker Interface - Device has the latest default image"); res = Ok(true); } else { - defmt::trace!("Kicker Interface - Device does not have the latest default image"); + defmt::trace!( + "Kicker Interface - Device does not have the latest default image" + ); res = Ok(false); } - }, + } Err(_) => { defmt::debug!("Kicker Interface - No device response, image hash unknown"); res = Err(()); - }, + } } // Make sure that the uart queue is empty of any possible parameter // response packets, which may cause side effects for the flashing @@ -249,15 +273,23 @@ impl< return res; } - pub async fn init_firmware_image(&mut self, flash: bool, fw_image_bytes: &[u8]) -> Result<(), ()> { + pub async fn init_firmware_image( + &mut self, + flash: bool, + fw_image_bytes: &[u8], + ) -> Result<(), ()> { if flash { defmt::info!("Kicker Interface - Flashing firmware image"); - self.stm32_uart_interface.load_firmware_image(fw_image_bytes, true).await?; + self.stm32_uart_interface + .load_firmware_image(fw_image_bytes, true) + .await?; } else { defmt::info!("Kicker Interface - Skipping firmware flash"); } - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; @@ -283,7 +315,7 @@ impl< } else { flash = true; } - }, + } Err(_) => { flash = true; } @@ -291,4 +323,4 @@ impl< } return self.init_firmware_image(flash, self.firmware_image).await; } -} \ No newline at end of file +} diff --git a/control-board/src/drivers/mod.rs b/control-board/src/drivers/mod.rs index 1cae09b3..2ccf14fa 100644 --- a/control-board/src/drivers/mod.rs +++ b/control-board/src/drivers/mod.rs @@ -1,3 +1,3 @@ pub mod kicker; pub mod radio_robot; -pub mod shell_indicator; \ No newline at end of file +pub mod shell_indicator; diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index e18c9e03..1d5853f9 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -1,16 +1,19 @@ -use ateam_lib_stm32::drivers::radio::odin_w26x::{OdinRadioError, OdinW262, PeerConnection, WifiAuth}; -use ateam_lib_stm32::uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings::{ - self, BasicControl, BasicTelemetry, CommandCode, ErrorTelemetry, ExtendedTelemetry, HelloRequest, HelloResponse, ParameterCommand, RadioPacket, RadioPacket_Data + self, BasicControl, BasicTelemetry, CommandCode, ErrorTelemetry, ExtendedTelemetry, + HelloRequest, HelloResponse, ParameterCommand, RadioPacket, RadioPacket_Data, }; use ateam_common_packets::radio::DataPacket; +use ateam_lib_stm32::drivers::radio::odin_w26x::{ + OdinRadioError, OdinW262, PeerConnection, WifiAuth, +}; +use ateam_lib_stm32::uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}; use const_format::formatcp; -use credentials::WifiCredential; -use embassy_stm32::uid; use core::fmt::Write; use core::mem::size_of; +use credentials::WifiCredential; use embassy_futures::select::{select, Either}; -use embassy_stm32::gpio::{Level, Pin, Speed, Output}; +use embassy_stm32::gpio::{Level, Output, Pin, Speed}; +use embassy_stm32::uid; use embassy_stm32::usart::{self, DataBits, Parity, StopBits}; use embassy_time::{Duration, Timer}; use heapless::String; @@ -25,7 +28,7 @@ const LOCAL_PORT: u16 = 42069; pub enum WifiNetwork { Team, CompMain, - CompPractice + CompPractice, } #[derive(Copy, Clone)] @@ -78,8 +81,9 @@ unsafe impl< const DEPTH_TX: usize, const DEPTH_RX: usize, const DEBUG_UART_QUEUES: bool, - > Send for RobotRadio<'a, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX, DEBUG_UART_QUEUES> {} - + > Send for RobotRadio<'a, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX, DEBUG_UART_QUEUES> +{ +} pub struct RobotRadio< 'a, @@ -89,14 +93,7 @@ pub struct RobotRadio< const DEPTH_RX: usize, const DEBUG_UART_QUEUES: bool, > { - odin_driver: OdinW262< - 'a, - LEN_TX, - LEN_RX, - DEPTH_TX, - DEPTH_RX, - DEBUG_UART_QUEUES, - >, + odin_driver: OdinW262<'a, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX, DEBUG_UART_QUEUES>, reset_pin: Output<'a>, use_flow_control: bool, peer: Option, @@ -152,7 +149,12 @@ impl< pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { // were about to reset the radio, so we also need to reset the uart queue config to match the startup config - if self.odin_driver.update_host_uart_config(self.get_startup_uart_config()).await.is_err() { + if self + .odin_driver + .update_host_uart_config(self.get_startup_uart_config()) + .await + .is_err() + { defmt::debug!("failed to reset host uart to startup config."); } @@ -174,21 +176,29 @@ impl< defmt::debug!("error disabling echo on radio"); return Err(RobotRadioError::ConnectUartBadEcho); } - - if self.odin_driver.config_uart(baudrate, self.use_flow_control, 8, true).await.is_err() { + + if self + .odin_driver + .config_uart(baudrate, self.use_flow_control, 8, true) + .await + .is_err() + { defmt::debug!("error increasing radio baud rate."); return Err(RobotRadioError::ConnectUartBadRadioConfigUpdate); } defmt::trace!("configured radio link speed"); - - if self.odin_driver.update_host_uart_config(self.get_highspeed_uart_config()).await.is_err() { + if self + .odin_driver + .update_host_uart_config(self.get_highspeed_uart_config()) + .await + .is_err() + { defmt::debug!("error increasing host baud rate."); return Err(RobotRadioError::ConnectUartBadHostConfigUpdate); } defmt::trace!("configured host link speed"); - // Datasheet says wait at least 40ms after UART config change Timer::after(Duration::from_millis(50)).await; @@ -196,7 +206,7 @@ impl< if let Ok(got_edm_startup) = self.odin_driver.enter_edm().await { defmt::trace!("entered edm at high link speed"); - if ! got_edm_startup { + if !got_edm_startup { if self.odin_driver.wait_edm_startup().await.is_err() { defmt::debug!("error waiting for EDM startup after uart baudrate increase"); return Err(RobotRadioError::ConnectUartNoEdmStartup); @@ -206,7 +216,7 @@ impl< } } else { defmt::debug!("error entering EDM mode after uart baudrate increase"); - return Err(RobotRadioError::ConnectUartCannotEnterEdm) + return Err(RobotRadioError::ConnectUartCannotEnterEdm); } Timer::after(Duration::from_millis(50)).await; @@ -241,13 +251,23 @@ impl< } } - pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential, robot_number: u8) -> Result<(), RobotRadioError> { + pub async fn connect_to_network( + &mut self, + wifi_credential: WifiCredential, + robot_number: u8, + ) -> Result<(), RobotRadioError> { // set radio hardware name enumeration let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; let mut s = String::<25>::new(); - core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); + core::write!( + &mut s, + "A-Team Robot #{:02X} ({:04X})", + robot_number, + uid_u16 + ) + .unwrap(); // let mut s = String::<16>::new(); // core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); if self.odin_driver.set_host_name(s.as_str()).await.is_err() { @@ -260,7 +280,12 @@ impl< let wifi_pass = WifiAuth::WPA { passphrase: wifi_credential.get_password(), }; - if self.odin_driver.config_wifi(1, wifi_ssid, wifi_pass).await.is_err() { + if self + .odin_driver + .config_wifi(1, wifi_ssid, wifi_pass) + .await + .is_err() + { defmt::trace!("could not configure wifi profile"); return Err(RobotRadioError::ConnectWifiBadConfig); } @@ -282,7 +307,9 @@ impl< } pub async fn open_multicast(&mut self) -> Result<(), RobotRadioError> { - let peer = self.odin_driver.connect_peer(formatcp!( + let peer = self + .odin_driver + .connect_peer(formatcp!( "udp://{MULTICAST_IP}:{MULTICAST_PORT}/?flags=1&local_port={LOCAL_PORT}" )) .await; @@ -352,13 +379,13 @@ impl< if self.peer.is_some() { if self.odin_driver.can_read_data() { match self.odin_driver.try_read_data(fn_read) { - Ok(ret) => { - Ok(Some(ret)) - }, + Ok(ret) => Ok(Some(ret)), Err(e) => { - defmt::trace!("try read data failed after can read data reported data ready"); + defmt::trace!( + "try read data failed after can read data reported data ready" + ); Err(RobotRadioError::DriverError(e)) - }, + } } } else { Ok(None) @@ -449,7 +476,7 @@ impl< command_code: CommandCode::CC_TELEMETRY, data_length: size_of::() as u16, data: RadioPacket_Data { - telemetry: telemetry + telemetry: telemetry, }, }; let packet_bytes = unsafe { @@ -464,7 +491,10 @@ impl< Ok(()) } - pub async fn send_control_debug_telemetry(&self, telemetry: ExtendedTelemetry) -> Result<(), RobotRadioError> { + pub async fn send_control_debug_telemetry( + &self, + telemetry: ExtendedTelemetry, + ) -> Result<(), RobotRadioError> { let packet = RadioPacket { crc32: 0, major_version: bindings::kProtocolVersionMajor, @@ -472,7 +502,7 @@ impl< command_code: CommandCode::CC_CONTROL_DEBUG_TELEMETRY, data_length: size_of::() as u16, data: RadioPacket_Data { - control_debug_telemetry: telemetry + control_debug_telemetry: telemetry, }, }; let packet_bytes = unsafe { @@ -487,7 +517,10 @@ impl< Ok(()) } - pub async fn send_parameter_response(&self, parameter_cmd: ParameterCommand) -> Result<(), RobotRadioError> { + pub async fn send_parameter_response( + &self, + parameter_cmd: ParameterCommand, + ) -> Result<(), RobotRadioError> { let packet = RadioPacket { crc32: 0, major_version: bindings::kProtocolVersionMajor, @@ -495,7 +528,7 @@ impl< command_code: CommandCode::CC_ROBOT_PARAMETER_COMMAND, data_length: size_of::() as u16, data: RadioPacket_Data { - robot_parameter_command: parameter_cmd + robot_parameter_command: parameter_cmd, }, }; let packet_bytes = unsafe { @@ -510,16 +543,17 @@ impl< Ok(()) } - pub async fn send_error_telemetry(&self, error_telemetry: ErrorTelemetry) -> Result<(), RobotRadioError> { + pub async fn send_error_telemetry( + &self, + error_telemetry: ErrorTelemetry, + ) -> Result<(), RobotRadioError> { let packet = RadioPacket { crc32: 0, major_version: bindings::kProtocolVersionMajor, minor_version: bindings::kProtocolVersionMinor, command_code: CommandCode::CC_ERROR_TELEMETRY, data_length: size_of::() as u16, - data: RadioPacket_Data { - error_telemetry - }, + data: RadioPacket_Data { error_telemetry }, }; let packet_bytes = unsafe { core::slice::from_raw_parts( @@ -561,9 +595,10 @@ impl< } pub fn parse_data_packet(&self, data: &[u8]) -> Result { - const CONTROL_PACKET_SIZE: usize = size_of::() - size_of::() - + size_of::(); - const PARAMERTER_PACKET_SIZE: usize = size_of::() - size_of::() + const CONTROL_PACKET_SIZE: usize = + size_of::() - size_of::() + size_of::(); + const PARAMERTER_PACKET_SIZE: usize = size_of::() + - size_of::() + size_of::(); if data.len() == CONTROL_PACKET_SIZE { @@ -594,16 +629,11 @@ impl< } pub async fn read_packet(&self) -> Result { - self.read_data(|data| { - self.parse_data_packet(data) - }) - .await? + self.read_data(|data| self.parse_data_packet(data)).await? } pub fn read_packet_nonblocking(&self) -> Result, ()> { - let res = self.read_data_nonblocking(|data| { - self.parse_data_packet(data) - }); + let res = self.read_data_nonblocking(|data| self.parse_data_packet(data)); match res { Ok(res) => { @@ -612,25 +642,23 @@ impl< match pkt { Ok(pkt) => { return Ok(Some(pkt)); - }, + } Err(_) => { // we got data that was a valid EDM DataPacket, but couldn't parse it // into any known A-Team packet format defmt::debug!("got EDM packet but wasn't A-Team"); return Err(()); - }, + } } - }, - None => { - return Ok(None) - }, + } + None => return Ok(None), } - }, + } Err(err_res) => { defmt::debug!("radio in invalid state {:?}", err_res); // read_data_nonblocking failed because the radio was in an invalid state - return Err(()) - }, + return Err(()); + } } } } diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index dc24b40a..27d35c08 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -19,7 +19,13 @@ impl<'a> ShellIndicator<'a> { ]; // TODO: refactor pin ordering - pub fn new(fr_pin0: impl Pin, fl_pin1: impl Pin, br_pin2: impl Pin, bl_pin3: impl Pin, team_pin4: Option) -> Self { + pub fn new( + fr_pin0: impl Pin, + fl_pin1: impl Pin, + br_pin2: impl Pin, + bl_pin3: impl Pin, + team_pin4: Option, + ) -> Self { let team_pin4: Option> = if let Some(pin4) = team_pin4 { Some(Output::new(pin4, Level::Low, Speed::Low)) } else { diff --git a/control-board/src/image_hash.rs b/control-board/src/image_hash.rs index c4df5ecb..48a132c8 100644 --- a/control-board/src/image_hash.rs +++ b/control-board/src/image_hash.rs @@ -2,20 +2,20 @@ use core::ptr::read_volatile; struct ImgHash { #[allow(dead_code)] - img_hash_magic: [u8; 16], // Used to find this struct in binary - img_hash: [u8; 16] // Hash is injected after compilation + img_hash_magic: [u8; 16], // Used to find this struct in binary + img_hash: [u8; 16], // Hash is injected after compilation } /// Wheel image hash, saved in the control image static mut WHEEL_IMG_HASH: ImgHash = ImgHash { img_hash_magic: *b"WheelImgHashCtrl", - img_hash: [0; 16] + img_hash: [0; 16], }; /// Kicker image hash, saved in the control image static mut KICKER_IMG_HASH: ImgHash = ImgHash { img_hash_magic: *b"KickrImgHashCtrl", - img_hash: [0; 16] + img_hash: [0; 16], }; pub fn get_wheel_img_hash() -> [u8; 16] { @@ -30,4 +30,4 @@ pub fn get_kicker_img_hash() -> [u8; 16] { unsafe { return read_volatile(&raw const KICKER_IMG_HASH.img_hash as *const [u8; 16]); } -} \ No newline at end of file +} diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index d12cccd7..031364c2 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -1,10 +1,8 @@ #![no_std] #![no_main] - #![allow(incomplete_features)] #![feature(inherent_associated_types)] #![feature(generic_const_exprs)] - #![feature(type_alias_impl_trait)] #![feature(async_closure)] #![feature( @@ -16,26 +14,28 @@ #![feature(sync_unsafe_cell)] #![feature(variant_count)] - -use ateam_common_packets::{bindings::{ErrorTelemetry, ParameterDataFormat}, radio::DataPacket}; +use ateam_common_packets::{ + bindings::{ErrorTelemetry, ParameterDataFormat}, + radio::DataPacket, +}; use embassy_stm32::{ - bind_interrupts, peripherals, rcc::{ + bind_interrupts, peripherals, + rcc::{ mux::{Adcsel, Saisel, Sdmmcsel, Spi6sel, Usart16910sel, Usart234578sel, Usbsel}, - AHBPrescaler, APBPrescaler, - Hse, HseMode, - Pll, PllDiv, PllMul, PllPreDiv, PllSource, - Sysclk, - VoltageScale - }, time::Hertz, usart, Config + AHBPrescaler, APBPrescaler, Hse, HseMode, Pll, PllDiv, PllMul, PllPreDiv, PllSource, + Sysclk, VoltageScale, + }, + time::Hertz, + usart, Config, }; use embassy_time::Instant; +pub mod image_hash; pub mod parameter_interface; pub mod pins; pub mod robot_state; pub mod songs; pub mod stspin_motor; -pub mod image_hash; pub mod drivers; pub mod motion; @@ -72,34 +72,50 @@ pub enum MotorIndex { pub mod colors { use smart_leds::RGB8; - pub const COLOR_OFF: RGB8 = RGB8 { r: 0, g: 0, b: 0}; - pub const COLOR_RED: RGB8 = RGB8 { r: 10, g: 0, b: 0}; - pub const COLOR_YELLOW: RGB8 = RGB8 { r: 10, g: 10, b: 0}; - pub const COLOR_GREEN: RGB8 = RGB8 { r: 0, g: 10, b: 0}; - pub const COLOR_BLUE: RGB8 = RGB8 { r: 0, g: 0, b: 10}; + pub const COLOR_OFF: RGB8 = RGB8 { r: 0, g: 0, b: 0 }; + pub const COLOR_RED: RGB8 = RGB8 { r: 10, g: 0, b: 0 }; + pub const COLOR_YELLOW: RGB8 = RGB8 { r: 10, g: 10, b: 0 }; + pub const COLOR_GREEN: RGB8 = RGB8 { r: 0, g: 10, b: 0 }; + pub const COLOR_BLUE: RGB8 = RGB8 { r: 0, g: 0, b: 10 }; } #[macro_export] macro_rules! include_external_cpp_bin { ($var_name:ident, $bin_file:literal) => { - pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../motor-controller/build/bin/", $bin_file)).len()] - = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../motor-controller/build/bin/", $bin_file)); - } + pub static $var_name: &[u8; include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../motor-controller/build/bin/", + $bin_file + )) + .len()] = include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../motor-controller/build/bin/", + $bin_file + )); + }; } #[macro_export] macro_rules! include_kicker_bin { ($var_name:ident, $bin_file:literal) => { - pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabihf/release/", $bin_file)).len()] - = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabihf/release/", $bin_file)); - } + pub static $var_name: &[u8; include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../kicker-board/target/thumbv7em-none-eabihf/release/", + $bin_file + )) + .len()] = include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../kicker-board/target/thumbv7em-none-eabihf/release/", + $bin_file + )); + }; } pub const BATTERY_MIN_SAFE_VOLTAGE: f32 = 21.0; pub const BATTERY_MIN_CRIT_VOLTAGE: f32 = 19.5; pub const BATTERY_MAX_VOLTAGE: f32 = 26.0; pub const BATTERY_BUFFER_SIZE: usize = 20; -pub const ADC_TO_BATTERY_DIVIDER: f32 = (11_500.0 + 1_000.0) / 1_000.0; +pub const ADC_TO_BATTERY_DIVIDER: f32 = (11_500.0 + 1_000.0) / 1_000.0; pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { adc_v * ADC_TO_BATTERY_DIVIDER @@ -127,10 +143,10 @@ pub fn get_system_config() -> Config { config.rcc.pll1 = Some(Pll { source: PllSource::HSE, prediv: PllPreDiv::DIV1, - mul: PllMul::MUL64, // PllMul::MUL68, + mul: PllMul::MUL64, // PllMul::MUL68, divp: Some(PllDiv::DIV1), // 544 MHz divq: Some(PllDiv::DIV4), // 136 MHz - divr: Some(PllDiv::DIV2) // 272 MHz + divr: Some(PllDiv::DIV2), // 272 MHz }); config.rcc.pll2 = Some(Pll { source: PllSource::HSE, @@ -138,7 +154,7 @@ pub fn get_system_config() -> Config { mul: PllMul::MUL31, divp: Some(PllDiv::DIV7), // 35.8 MHz divq: Some(PllDiv::DIV2), // 124 MHz - divr: Some(PllDiv::DIV1) // 248 MHz + divr: Some(PllDiv::DIV1), // 248 MHz }); config.rcc.pll3 = Some(Pll { source: PllSource::HSE, @@ -146,7 +162,7 @@ pub fn get_system_config() -> Config { mul: PllMul::MUL93, divp: Some(PllDiv::DIV2), // 186 Mhz divq: Some(PllDiv::DIV3), // 124 MHz - divr: Some(PllDiv::DIV3) // 124 MHz + divr: Some(PllDiv::DIV3), // 124 MHz }); // configure core busses @@ -168,7 +184,7 @@ pub fn get_system_config() -> Config { config.rcc.mux.usart16910sel = Usart16910sel::PCLK2; // 136 MHz config.rcc.mux.spi6sel = Spi6sel::PCLK4; // 136 MHz config.rcc.mux.sdmmcsel = Sdmmcsel::PLL2_R; // 248 MHz - // config.rcc.mux.adcsel = Adcsel::PLL3_R; // 124 MHz + // config.rcc.mux.adcsel = Adcsel::PLL3_R; // 124 MHz config.rcc.mux.adcsel = Adcsel::PLL2_P; config.rcc.mux.usbsel = Usbsel::PLL3_Q; // 124 MHz @@ -193,42 +209,40 @@ pub const fn is_float_safe(f: f32) -> bool { pub fn is_command_packet_safe(cmd_pck: DataPacket) -> bool { match cmd_pck { DataPacket::BasicControl(basic_control) => { - is_float_safe(basic_control.vel_x_linear) && - is_float_safe(basic_control.vel_y_linear) && - is_float_safe(basic_control.vel_z_angular) && - is_float_safe(basic_control.kick_vel) && - is_float_safe(basic_control.dribbler_speed) - }, - DataPacket::ParameterCommand(parameter_command) => { - match parameter_command.data_format { - ParameterDataFormat::F32 => { - let float = unsafe { parameter_command.data.f32_ }; - return is_float_safe(float); - }, - ParameterDataFormat::MATRIX_F32 => { - let arr = unsafe { parameter_command.data.matrix_f32 }; - return is_float_array_safe(&arr); - }, - ParameterDataFormat::PID_F32 => { - let arr = unsafe { parameter_command.data.pid_f32 }; - return is_float_array_safe(&arr); - }, - ParameterDataFormat::PID_LIMITED_INTEGRAL_F32 => { - let arr = unsafe { parameter_command.data.pidii_f32 }; - return is_float_array_safe(&arr); - }, - ParameterDataFormat::VEC3_F32 => { - let arr = unsafe { parameter_command.data.vec3_f32 }; - return is_float_array_safe(&arr); - }, - ParameterDataFormat::VEC4_F32 => { - let arr = unsafe { parameter_command.data.vec4_f32 }; - return is_float_array_safe(&arr); - }, - _ => { - defmt::error!("Parameter Command data packet has an unexpected data type"); - return false - }, + is_float_safe(basic_control.vel_x_linear) + && is_float_safe(basic_control.vel_y_linear) + && is_float_safe(basic_control.vel_z_angular) + && is_float_safe(basic_control.kick_vel) + && is_float_safe(basic_control.dribbler_speed) + } + DataPacket::ParameterCommand(parameter_command) => match parameter_command.data_format { + ParameterDataFormat::F32 => { + let float = unsafe { parameter_command.data.f32_ }; + return is_float_safe(float); + } + ParameterDataFormat::MATRIX_F32 => { + let arr = unsafe { parameter_command.data.matrix_f32 }; + return is_float_array_safe(&arr); + } + ParameterDataFormat::PID_F32 => { + let arr = unsafe { parameter_command.data.pid_f32 }; + return is_float_array_safe(&arr); + } + ParameterDataFormat::PID_LIMITED_INTEGRAL_F32 => { + let arr = unsafe { parameter_command.data.pidii_f32 }; + return is_float_array_safe(&arr); + } + ParameterDataFormat::VEC3_F32 => { + let arr = unsafe { parameter_command.data.vec3_f32 }; + return is_float_array_safe(&arr); + } + ParameterDataFormat::VEC4_F32 => { + let arr = unsafe { parameter_command.data.vec4_f32 }; + return is_float_array_safe(&arr); + } + _ => { + defmt::error!("Parameter Command data packet has an unexpected data type"); + return false; } }, } @@ -239,6 +253,6 @@ pub fn create_error_telemetry_from_string(error_message: &str) -> ErrorTelemetry let bytes = error_message.as_bytes(); let copy_len = core::cmp::min(bytes.len(), 60); error_telemetry.error_message[..copy_len].copy_from_slice(&bytes[..copy_len]); - error_telemetry.timestamp = Instant::now().as_millis() as u32; // Overflows after ~50 days of uptime + error_telemetry.timestamp = Instant::now().as_millis() as u32; // Overflows after ~50 days of uptime error_telemetry -} \ No newline at end of file +} diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs index 83a7fc24..24ae921c 100644 --- a/control-board/src/motion/constant_gain_kalman_filter.rs +++ b/control-board/src/motion/constant_gain_kalman_filter.rs @@ -1,6 +1,11 @@ use nalgebra::base::SMatrix; -pub struct CgKalmanFilter<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBSERVATIONS: usize> { +pub struct CgKalmanFilter< + 'a, + const NUM_STATES: usize, + const NUM_CONTROL_INPUTS: usize, + const NUM_OBSERVATIONS: usize, +> { state_transition: &'a SMatrix, control_input: &'a SMatrix, observation_model: &'a SMatrix, @@ -10,17 +15,24 @@ pub struct CgKalmanFilter<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: state_estimate: SMatrix, pred_state_estimate: SMatrix, pred_estimate_cov: SMatrix, - measurement_residual: SMatrix + measurement_residual: SMatrix, } -impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBSERVATIONS: usize> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { - pub fn new(state_transition: &'a SMatrix, - control_input: &'a SMatrix, - observation_model: &'a SMatrix, - process_cov: &'a SMatrix, - kalman_gain: &'a SMatrix, - estimate_cov: &'a SMatrix::, - ) -> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { +impl< + 'a, + const NUM_STATES: usize, + const NUM_CONTROL_INPUTS: usize, + const NUM_OBSERVATIONS: usize, + > CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> +{ + pub fn new( + state_transition: &'a SMatrix, + control_input: &'a SMatrix, + observation_model: &'a SMatrix, + process_cov: &'a SMatrix, + kalman_gain: &'a SMatrix, + estimate_cov: &'a SMatrix, + ) -> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { let mut filter = CgKalmanFilter { state_transition: state_transition, control_input: control_input, @@ -31,7 +43,7 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS state_estimate: SMatrix::::zeros(), pred_state_estimate: SMatrix::::zeros(), pred_estimate_cov: SMatrix::::zeros(), - measurement_residual: SMatrix::::zeros() + measurement_residual: SMatrix::::zeros(), }; // Just initializing to non-zero, so don't need a permanent reference in struct @@ -41,8 +53,11 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS } pub fn predict(&mut self, u: &SMatrix) { - self.pred_state_estimate = self.state_transition * self.state_estimate + self.control_input * u; - self.pred_estimate_cov = self.state_transition * self.estimate_cov * self.state_transition.transpose() + self.process_cov; + self.pred_state_estimate = + self.state_transition * self.state_estimate + self.control_input * u; + self.pred_estimate_cov = + self.state_transition * self.estimate_cov * self.state_transition.transpose() + + self.process_cov; } pub fn update(&mut self, measurement: &SMatrix) { @@ -51,7 +66,7 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS // Constant gain so don't need Innovation Covariance / Optimal Kalman gain calculation self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; - + self.measurement_residual = measurement - self.observation_model * self.state_estimate; } @@ -63,4 +78,3 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS self.measurement_residual } } - diff --git a/control-board/src/motion/mod.rs b/control-board/src/motion/mod.rs index 8a59a057..b7a8108b 100644 --- a/control-board/src/motion/mod.rs +++ b/control-board/src/motion/mod.rs @@ -3,4 +3,4 @@ pub mod pid; pub mod robot_controller; pub mod robot_model; -pub mod params; \ No newline at end of file +pub mod params; diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 1ad16de5..a1c0c980 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -4,51 +4,43 @@ pub const KF_NUM_STATES: usize = 3; pub const KF_NUM_CONTROL_INPUTS: usize = 4; pub const KF_NUM_OBSERVATIONS: usize = 5; -const EXPECTED_DT: f32 = 10000.0; // 10mS = 10000uS +const EXPECTED_DT: f32 = 10000.0; // 10mS = 10000uS const EXPECTED_DT_2: f32 = EXPECTED_DT * EXPECTED_DT; -const ENCODER_NOISE: f32 = 0.11; // noise in rad / sampling time -const GYRO_NOISE: f32 = 0.002; // BMI085 compensated for spectral noise at 100Hz sample rate +const ENCODER_NOISE: f32 = 0.11; // noise in rad / sampling time +const GYRO_NOISE: f32 = 0.002; // BMI085 compensated for spectral noise at 100Hz sample rate const PROCESS_NOISE: f32 = 0.05; const INITIAL_COV: f32 = 0.11; - // Assume constant velocity as a valid linearization of the transition system -pub static STATE_TRANSITION: Matrix3 = - matrix![1.0, 0.0, 0.0; +pub static STATE_TRANSITION: Matrix3 = matrix![1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0]; // Assume no input for the moment -pub static CONTROL_INPUT: Matrix3x4 = - matrix![0.0, 0.0, 0.0, 0.0; +pub static CONTROL_INPUT: Matrix3x4 = matrix![0.0, 0.0, 0.0, 0.0; 0.0, 0.0, 0.0, 0.0; 0.0, 0.0, 0.0, 0.0]; -pub static OBSERVATION_MODEL: Matrix5x3 = - matrix![-28.86751346, 16.66666667, 2.71333333; +pub static OBSERVATION_MODEL: Matrix5x3 = matrix![-28.86751346, 16.66666667, 2.71333333; -23.57022604, -23.57022604, 2.71333333; 23.57022604, -23.57022604, 2.71333333; 28.86751346, 16.66666667, 2.71333333; 0.0, 0.0, 1.0 ]; - -pub static PROCESS_COV: Matrix3 = - matrix![KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0, 0.0; + +pub static PROCESS_COV: Matrix3 = matrix![KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0, 0.0; 0.0, KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0; 0.0, 0.0, KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2]; -pub static OBSERVATION_COV: Matrix5 = - matrix![ENCODER_NOISE, 0.0, 0.0, 0.0, 0.0; +pub static OBSERVATION_COV: Matrix5 = matrix![ENCODER_NOISE, 0.0, 0.0, 0.0, 0.0; 0.0, ENCODER_NOISE, 0.0, 0.0, 0.0; 0.0, 0.0, ENCODER_NOISE, 0.0, 0.0; 0.0, 0.0, 0.0, ENCODER_NOISE, 0.0; 0.0, 0.0, 0.0, 0.0, GYRO_NOISE]; -pub static INIT_ESTIMATE_COV: Matrix3 = - matrix![INITIAL_COV, 0.0, 0.0; +pub static INIT_ESTIMATE_COV: Matrix3 = matrix![INITIAL_COV, 0.0, 0.0; 0.0, INITIAL_COV, 0.0; 0.0, 0.0, INITIAL_COV]; -pub static KALMAN_GAIN: Matrix3x5 = - matrix![-1.03923044e-02, -8.48528101e-03, 8.48528101e-03, 1.03923044e-02, -8.82095698e-11; +pub static KALMAN_GAIN: Matrix3x5 = matrix![-1.03923044e-02, -8.48528101e-03, 8.48528101e-03, 1.03923044e-02, -8.82095698e-11; 1.05519794e-02, -1.37518257e-02, -1.37518257e-02, 1.05519794e-02, 1.73644781e-02; - 2.45564740e-02, 1.73640497e-02, 1.73640497e-02, 2.45564740e-02, 7.72510348e-01 ]; \ No newline at end of file + 2.45564740e-02, 1.73640497e-02, 1.73640497e-02, 2.45564740e-02, 7.72510348e-01 ]; diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 9458f820..3ce13112 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -1,22 +1,21 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; // Kp, Ki, Kd, Ki_err_min, Ki_err_max -pub static PID_GAIN: Matrix3x5 = - matrix![0.0, 0.0, 0.0, -1.0, 1.0; +pub static PID_GAIN: Matrix3x5 = matrix![0.0, 0.0, 0.0, -1.0, 1.0; 0.0, 0.0, 0.0, -1.0, 1.0; 0.0, 0.0, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) // NOTE: Because of (bad) motor model, need to give way // higher constraints to allow controller to overcome -// static friction / loading. +// static friction / loading. // 8, 8, 34.9 maxes out motors/IMU measurement rate -pub static BODY_VEL_LIM: Vector3 = vector![8.0, 8.0, 40.0]; -pub static BODY_ACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore -pub static BODY_DEACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore +pub static BODY_VEL_LIM: Vector3 = vector![8.0, 8.0, 40.0]; +pub static BODY_ACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore +pub static BODY_DEACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) -// Rough estimate for peak rating +// Rough estimate for peak rating // alpha = rated torque / interia // 8.4 Ncm^2 / (270 gcm^2) = 3110 rad/s^2 actual max -pub static WHEEL_ACC_LIM: Vector4 = vector![2000.0, 2000.0, 2000.0, 2000.0]; \ No newline at end of file +pub static WHEEL_ACC_LIM: Vector4 = vector![2000.0, 2000.0, 2000.0, 2000.0]; diff --git a/control-board/src/motion/params/mod.rs b/control-board/src/motion/params/mod.rs index 23d4d6a7..1a08f5d9 100644 --- a/control-board/src/motion/params/mod.rs +++ b/control-board/src/motion/params/mod.rs @@ -1,4 +1,4 @@ pub mod body_vel_filter_params; pub mod body_vel_pid_params; -pub mod robot_physical_params; \ No newline at end of file +pub mod robot_physical_params; diff --git a/control-board/src/motion/params/robot_physical_params.rs b/control-board/src/motion/params/robot_physical_params.rs index 0b887525..741e5655 100644 --- a/control-board/src/motion/params/robot_physical_params.rs +++ b/control-board/src/motion/params/robot_physical_params.rs @@ -1,4 +1,4 @@ use nalgebra::Vector4; pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(330.0, 45.0, 135.0, 210.0); // Degrees from y+ pub const WHEEL_RADIUS_M: f32 = 0.030; // was 0.0247 // wheel dia 49mm -pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot \ No newline at end of file +pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot diff --git a/control-board/src/motion/pid.rs b/control-board/src/motion/pid.rs index ac17c94b..a49b88f6 100644 --- a/control-board/src/motion/pid.rs +++ b/control-board/src/motion/pid.rs @@ -1,6 +1,6 @@ use nalgebra::{ base::{SMatrix, SVector}, - clamp + clamp, }; pub struct PidController { @@ -12,20 +12,29 @@ pub struct PidController { impl<'a, const NUM_STATES: usize> PidController { pub fn from_gains_matrix(gain: &'a SMatrix) -> PidController { PidController { - gain: gain.clone(), + gain: gain.clone(), prev_error: SVector::::zeros(), integrated_error: SVector::::zeros(), } } - pub fn calculate(&mut self, setpoint: &SVector, process_variable: &SVector, dt_s: f32) -> SVector { + pub fn calculate( + &mut self, + setpoint: &SVector, + process_variable: &SVector, + dt_s: f32, + ) -> SVector { let error = setpoint - process_variable; // Calculate integrated error. let cur_integrated_error = self.integrated_error + (error * dt_s); // clamp error - // is there a better way to do this? - self.integrated_error = cur_integrated_error.zip_zip_map(&self.gain.column(3), &self.gain.column(4), |err, min_err, max_err| clamp(err, min_err, max_err)); + // is there a better way to do this? + self.integrated_error = cur_integrated_error.zip_zip_map( + &self.gain.column(3), + &self.gain.column(4), + |err, min_err, max_err| clamp(err, min_err, max_err), + ); // calculate derivative error let de_dt = (error - self.prev_error) / dt_s; @@ -34,7 +43,7 @@ impl<'a, const NUM_STATES: usize> PidController { let p = self.gain.column(0).component_mul(&error); let i = self.gain.column(1).component_mul(&self.integrated_error); let d = self.gain.column(2).component_mul(&de_dt); - + p + i + d } @@ -45,4 +54,4 @@ impl<'a, const NUM_STATES: usize> PidController { pub fn set_gain(&mut self, new_gain: SMatrix) { self.gain.copy_from(&new_gain) } -} \ No newline at end of file +} diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 96ef6019..537c5748 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -1,34 +1,38 @@ +use crate::parameter_interface::ParameterInterface; use ateam_common_packets::bindings::{ ParameterCommandCode::*, - ParameterName, ParameterDataFormat::{PID_LIMITED_INTEGRAL_F32, VEC3_F32, VEC4_F32}, - ParameterName::{VEL_PID_X, RC_BODY_VEL_LIMIT, RC_BODY_ACC_LIMIT, VEL_PID_Y, ANGULAR_VEL_PID_Z, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_PROCESS_NOISE, VEL_CGKF_GYRO_NOISE, VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_K_MATRIX, RC_WHEEL_ACC_LIMIT}}; + ParameterName, + ParameterName::{ + ANGULAR_VEL_PID_Z, RC_BODY_ACC_LIMIT, RC_BODY_VEL_LIMIT, RC_WHEEL_ACC_LIMIT, + VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_GYRO_NOISE, + VEL_CGKF_K_MATRIX, VEL_CGKF_PROCESS_NOISE, VEL_PID_X, VEL_PID_Y, + }, +}; use embassy_stm32::pac::adc::vals::Exten; use nalgebra::{SVector, Vector3, Vector4, Vector5}; -use crate::parameter_interface::ParameterInterface; use super::constant_gain_kalman_filter::CgKalmanFilter; use super::pid::PidController; use super::robot_model::RobotModel; use super::params::body_vel_filter_params::{ - KF_NUM_STATES, KF_NUM_CONTROL_INPUTS, KF_NUM_OBSERVATIONS, - STATE_TRANSITION, CONTROL_INPUT, OBSERVATION_MODEL, PROCESS_COV, INIT_ESTIMATE_COV, KALMAN_GAIN, + CONTROL_INPUT, INIT_ESTIMATE_COV, KALMAN_GAIN, KF_NUM_CONTROL_INPUTS, KF_NUM_OBSERVATIONS, + KF_NUM_STATES, OBSERVATION_MODEL, PROCESS_COV, STATE_TRANSITION, }; use super::params::body_vel_pid_params::{ - PID_GAIN, - BODY_VEL_LIM, BODY_ACC_LIM, BODY_DEACC_LIM, WHEEL_ACC_LIM + BODY_ACC_LIM, BODY_DEACC_LIM, BODY_VEL_LIM, PID_GAIN, WHEEL_ACC_LIM, }; -use ateam_common_packets::bindings::{ - ExtendedTelemetry, - ParameterCommand -}; +use ateam_common_packets::bindings::{ExtendedTelemetry, ParameterCommand}; // TODO find some general numeric type trait(s) for D // Clamp the vector, but keep the direction consistent. -pub fn clamp_vector_keep_dir(vec: &SVector, limits_abs: &SVector) -> SVector { +pub fn clamp_vector_keep_dir( + vec: &SVector, + limits_abs: &SVector, +) -> SVector { // Applies the clamping in a sign-less way by getting the scale of the vector compared to absolute value clamp region. let scales: SVector = vec.abs().component_div(limits_abs); // To maintain direction, do a scalar divison of the max of the scales. Limit to make sure to only scale down. @@ -50,9 +54,22 @@ pub struct BodyVelocityController<'a> { } impl<'a> BodyVelocityController<'a> { - pub fn new_from_global_params(loop_dt_s: f32, robot_model: RobotModel) -> BodyVelocityController<'a> { - let body_vel_filter: CgKalmanFilter = - CgKalmanFilter::new(&STATE_TRANSITION, &CONTROL_INPUT, &OBSERVATION_MODEL, &PROCESS_COV, &KALMAN_GAIN, &INIT_ESTIMATE_COV); + pub fn new_from_global_params( + loop_dt_s: f32, + robot_model: RobotModel, + ) -> BodyVelocityController<'a> { + let body_vel_filter: CgKalmanFilter< + KF_NUM_STATES, + KF_NUM_CONTROL_INPUTS, + KF_NUM_OBSERVATIONS, + > = CgKalmanFilter::new( + &STATE_TRANSITION, + &CONTROL_INPUT, + &OBSERVATION_MODEL, + &PROCESS_COV, + &KALMAN_GAIN, + &INIT_ESTIMATE_COV, + ); let body_vel_controller: PidController = PidController::from_gains_matrix(&PID_GAIN); @@ -79,14 +96,15 @@ impl<'a> BodyVelocityController<'a> { bvc } - pub fn new(loop_dt_s: f32, - robot_model: RobotModel, - body_vel_filter: CgKalmanFilter<'a, 3, 4, 5>, - pid_controller: PidController<3>, - bv_limit: Vector3, - ba_limit: Vector3, - bda_limit: Vector3, - wa_limit: Vector4 + pub fn new( + loop_dt_s: f32, + robot_model: RobotModel, + body_vel_filter: CgKalmanFilter<'a, 3, 4, 5>, + pid_controller: PidController<3>, + bv_limit: Vector3, + ba_limit: Vector3, + bda_limit: Vector3, + wa_limit: Vector4, ) -> BodyVelocityController<'a> { BodyVelocityController { loop_dt_s: loop_dt_s, @@ -103,14 +121,29 @@ impl<'a> BodyVelocityController<'a> { } } - pub fn control_update(&mut self, body_vel_setpoint: &Vector3, wheel_velocities: &Vector4, _wheel_torques: &Vector4, gyro_theta: f32, controls_enabled: bool) { + pub fn control_update( + &mut self, + body_vel_setpoint: &Vector3, + wheel_velocities: &Vector4, + _wheel_torques: &Vector4, + gyro_theta: f32, + controls_enabled: bool, + ) { // Assign telemetry data // TODO pass all of the gyro data up, not just theta self.debug_telemetry.imu_gyro[2] = gyro_theta; - self.debug_telemetry.commanded_body_velocity.copy_from_slice(body_vel_setpoint.as_slice()); + self.debug_telemetry + .commanded_body_velocity + .copy_from_slice(body_vel_setpoint.as_slice()); - let measurement: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); + let measurement: Vector5 = Vector5::new( + wheel_velocities[0], + wheel_velocities[1], + wheel_velocities[2], + wheel_velocities[3], + gyro_theta, + ); // Update measurements process observation input into CGKF. self.body_vel_filter.update(&measurement); @@ -131,16 +164,24 @@ impl<'a> BodyVelocityController<'a> { body_vel_estimate[2] = 0.0; } - self.debug_telemetry.cgkf_body_velocity_state_estimate.copy_from_slice(body_vel_estimate.as_slice()); + self.debug_telemetry + .cgkf_body_velocity_state_estimate + .copy_from_slice(body_vel_estimate.as_slice()); // Apply control policy. - let body_vel_control_pid = self.body_vel_controller.calculate(&body_vel_setpoint, &body_vel_estimate, self.loop_dt_s); + let body_vel_control_pid = self.body_vel_controller.calculate( + &body_vel_setpoint, + &body_vel_estimate, + self.loop_dt_s, + ); // Add the commanded setpoint as a feedforward component. let body_vel_output = body_vel_control_pid + body_vel_setpoint; // let body_vel_output = body_vel_setpoint; - self.debug_telemetry.body_velocity_u.copy_from_slice(body_vel_output.as_slice()); + self.debug_telemetry + .body_velocity_u + .copy_from_slice(body_vel_output.as_slice()); // Determine commanded body acceleration based on previous control output, and clamp and maintain the direction of acceleration. // NOTE: Using previous control output instead of estimate so that collision disturbances would not impact. @@ -161,19 +202,28 @@ impl<'a> BodyVelocityController<'a> { temp_accel_decel_holder[2] = self.body_deceleration_limit[2]; } - let body_acc_output_clamp = clamp_vector_keep_dir(&body_acc_output, &temp_accel_decel_holder); + let body_acc_output_clamp = + clamp_vector_keep_dir(&body_acc_output, &temp_accel_decel_holder); // Convert back to body velocity let body_vel_output_acc_clamp = self.prev_output + (body_acc_output_clamp * self.loop_dt_s); // Clamp and maintain direction of control body velocity. - let body_vel_output_full_clamp = clamp_vector_keep_dir(&body_vel_output_acc_clamp, &self.body_velocity_limit); - self.prev_output.copy_from_slice(body_vel_output_full_clamp.as_slice()); - self.debug_telemetry.clamped_commanded_body_velocity.copy_from_slice(body_vel_output_full_clamp.as_slice()); + let body_vel_output_full_clamp = + clamp_vector_keep_dir(&body_vel_output_acc_clamp, &self.body_velocity_limit); + self.prev_output + .copy_from_slice(body_vel_output_full_clamp.as_slice()); + self.debug_telemetry + .clamped_commanded_body_velocity + .copy_from_slice(body_vel_output_full_clamp.as_slice()); // Transform body velocity commands into the wheel velocity domain. - let wheel_vel_output = self.robot_model.robot_vel_to_wheel_vel(&body_vel_output_full_clamp); - self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output.as_slice()); + let wheel_vel_output = self + .robot_model + .robot_vel_to_wheel_vel(&body_vel_output_full_clamp); + self.debug_telemetry + .wheel_velocity_u + .copy_from_slice(wheel_vel_output.as_slice()); // Use control law adjusted value to predict the next cycle's state. self.body_vel_filter.predict(&wheel_vel_output); @@ -193,7 +243,9 @@ impl<'a> BodyVelocityController<'a> { self.cmd_wheel_velocities = wheel_vel_output; } else { self.cmd_wheel_velocities = self.robot_model.robot_vel_to_wheel_vel(&body_vel_setpoint); - self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output.as_slice()); + self.debug_telemetry + .wheel_velocity_u + .copy_from_slice(wheel_vel_output.as_slice()); } } @@ -215,13 +267,19 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { return match param_name { RC_BODY_VEL_LIMIT | RC_BODY_ACC_LIMIT | RC_WHEEL_ACC_LIMIT => true, VEL_PID_X | VEL_PID_Y | ANGULAR_VEL_PID_Z => true, - VEL_CGKF_ENCODER_NOISE | VEL_CGKF_PROCESS_NOISE | VEL_CGKF_GYRO_NOISE | VEL_CGFK_INITIAL_COVARIANCE | VEL_CGKF_K_MATRIX => true, + VEL_CGKF_ENCODER_NOISE + | VEL_CGKF_PROCESS_NOISE + | VEL_CGKF_GYRO_NOISE + | VEL_CGFK_INITIAL_COVARIANCE + | VEL_CGKF_K_MATRIX => true, _ => false, - } + }; } - - fn apply_command(&mut self, param_cmd: &ParameterCommand) -> Result { + fn apply_command( + &mut self, + param_cmd: &ParameterCommand, + ) -> Result { let mut reply_cmd = param_cmd.clone(); // if we haven't been given an actionable command code, ignore the call @@ -233,7 +291,9 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { // if we've been asked to apply a command we don't have a key for it // error out if !self.has_name(param_cmd.parameter_name) { - defmt::warn!("asked to apply a command with a parameter name not managed by this module"); + defmt::warn!( + "asked to apply a command with a parameter name not managed by this module" + ); reply_cmd.command_code = PCC_NACK_INVALID_NAME; return Err(*param_cmd); } @@ -259,7 +319,7 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { } reply_cmd.command_code = PCC_ACK; - }, + } VEL_PID_Y => { reply_cmd.data_format = PID_LIMITED_INTEGRAL_F32; @@ -273,7 +333,7 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { } reply_cmd.command_code = PCC_ACK; - }, + } ANGULAR_VEL_PID_Z => { reply_cmd.data_format = PID_LIMITED_INTEGRAL_F32; @@ -287,7 +347,7 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { } reply_cmd.command_code = PCC_ACK; - }, + } // VEL_CGKF_ENCODER_NOISE => { // }, @@ -307,20 +367,35 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { // set the type reply_cmd.data_format = VEC3_F32; // read back the data - unsafe { reply_cmd.data.vec3_f32.copy_from_slice(self.body_velocity_limit.as_slice()); } + unsafe { + reply_cmd + .data + .vec3_f32 + .copy_from_slice(self.body_velocity_limit.as_slice()); + } reply_cmd.command_code = PCC_ACK; - }, + } RC_BODY_ACC_LIMIT => { reply_cmd.data_format = VEC3_F32; - unsafe { reply_cmd.data.vec3_f32.copy_from_slice(self.body_acceleration_limit.as_slice()); } + unsafe { + reply_cmd + .data + .vec3_f32 + .copy_from_slice(self.body_acceleration_limit.as_slice()); + } reply_cmd.command_code = PCC_ACK; - }, + } RC_WHEEL_ACC_LIMIT => { reply_cmd.data_format = VEC4_F32; - unsafe { reply_cmd.data.vec4_f32.copy_from_slice(self.wheel_acceleration_limits.as_slice()); } + unsafe { + reply_cmd + .data + .vec4_f32 + .copy_from_slice(self.wheel_acceleration_limits.as_slice()); + } reply_cmd.command_code = PCC_ACK; - }, + } _ => { defmt::debug!("unimplemented key read in RobotController"); reply_cmd.command_code = PCC_NACK_INVALID_NAME; @@ -333,10 +408,11 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { // read data into matrix, modify the matrix row, then write the whole thing back let mut current_pid_gain = self.body_vel_controller.get_gain(); - current_pid_gain.row_mut(0).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + current_pid_gain + .row_mut(0) + .copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); self.body_vel_controller.set_gain(current_pid_gain); - // can't slice copy b/c backing storage is column-major // so a row slice isn't contiguous in backing memory and // therefore you can't do a slice copy @@ -354,11 +430,13 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } VEL_PID_Y => { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { let mut current_pid_gain = self.body_vel_controller.get_gain(); - current_pid_gain.row_mut(1).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + current_pid_gain + .row_mut(1) + .copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); self.body_vel_controller.set_gain(current_pid_gain); let updated_pid_gain = self.body_vel_controller.get_gain(); @@ -375,11 +453,13 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } ANGULAR_VEL_PID_Z => { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { let mut current_pid_gain = self.body_vel_controller.get_gain(); - current_pid_gain.row_mut(2).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + current_pid_gain + .row_mut(2) + .copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); self.body_vel_controller.set_gain(current_pid_gain); let updated_pid_gain = self.body_vel_controller.get_gain(); @@ -396,7 +476,7 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } // VEL_CGKF_ENCODER_NOISE => { // }, @@ -415,39 +495,60 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { RC_BODY_VEL_LIMIT => { if param_cmd.data_format == VEC3_F32 { // write the new data, then read it back into the reply - self.body_velocity_limit.as_mut_slice().copy_from_slice(unsafe { ¶m_cmd.data.vec3_f32 }); - unsafe { reply_cmd.data.vec3_f32.copy_from_slice(self.body_velocity_limit.as_slice()); } + self.body_velocity_limit + .as_mut_slice() + .copy_from_slice(unsafe { ¶m_cmd.data.vec3_f32 }); + unsafe { + reply_cmd + .data + .vec3_f32 + .copy_from_slice(self.body_velocity_limit.as_slice()); + } reply_cmd.command_code = PCC_ACK; } else { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } RC_BODY_ACC_LIMIT => { if param_cmd.data_format == VEC3_F32 { // write the new data, then read it back into the reply - self.body_acceleration_limit.as_mut_slice().copy_from_slice(unsafe { ¶m_cmd.data.vec3_f32 }); - unsafe { reply_cmd.data.vec3_f32.copy_from_slice(self.body_acceleration_limit.as_slice()); } + self.body_acceleration_limit + .as_mut_slice() + .copy_from_slice(unsafe { ¶m_cmd.data.vec3_f32 }); + unsafe { + reply_cmd + .data + .vec3_f32 + .copy_from_slice(self.body_acceleration_limit.as_slice()); + } reply_cmd.command_code = PCC_ACK; } else { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } RC_WHEEL_ACC_LIMIT => { if param_cmd.data_format == VEC4_F32 { // write the new data, then read it back into the reply - self.wheel_acceleration_limits.as_mut_slice().copy_from_slice(unsafe { ¶m_cmd.data.vec4_f32 }); - unsafe { reply_cmd.data.vec4_f32.copy_from_slice(self.wheel_acceleration_limits.as_slice()); } + self.wheel_acceleration_limits + .as_mut_slice() + .copy_from_slice(unsafe { ¶m_cmd.data.vec4_f32 }); + unsafe { + reply_cmd + .data + .vec4_f32 + .copy_from_slice(self.wheel_acceleration_limits.as_slice()); + } reply_cmd.command_code = PCC_ACK; } else { reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; return Err(reply_cmd); } - }, + } _ => { defmt::debug!("unimplemented key write in RobotController"); reply_cmd.command_code = PCC_NACK_INVALID_NAME; diff --git a/control-board/src/motion/robot_model.rs b/control-board/src/motion/robot_model.rs index 46eeb25c..b5b90345 100644 --- a/control-board/src/motion/robot_model.rs +++ b/control-board/src/motion/robot_model.rs @@ -1,5 +1,5 @@ +use libm::{cosf, sinf}; use nalgebra::{Matrix3x4, Matrix4x3, Vector3, Vector4}; -use libm::{sinf, cosf}; #[derive(Clone, Copy)] pub struct RobotConstants { @@ -23,10 +23,18 @@ impl RobotModel { let neg_r = -(&robot_constants.wheel_radius_m); Matrix4x3::new( - cosf(theta[0]) / neg_r[0], sinf(theta[0]) / neg_r[0], -wheel_dist[0] / neg_r[0], - cosf(theta[1]) / neg_r[1], sinf(theta[1]) / neg_r[1], -wheel_dist[1] / neg_r[1], - cosf(theta[2]) / neg_r[2], sinf(theta[2]) / neg_r[2], -wheel_dist[2] / neg_r[2], - cosf(theta[3]) / neg_r[3], sinf(theta[3]) / neg_r[3], -wheel_dist[3] / neg_r[3], + cosf(theta[0]) / neg_r[0], + sinf(theta[0]) / neg_r[0], + -wheel_dist[0] / neg_r[0], + cosf(theta[1]) / neg_r[1], + sinf(theta[1]) / neg_r[1], + -wheel_dist[1] / neg_r[1], + cosf(theta[2]) / neg_r[2], + sinf(theta[2]) / neg_r[2], + -wheel_dist[2] / neg_r[2], + cosf(theta[3]) / neg_r[3], + sinf(theta[3]) / neg_r[3], + -wheel_dist[3] / neg_r[3], ) } @@ -36,7 +44,7 @@ impl RobotModel { let ta = t_body_to_wheel.transpose() * t_body_to_wheel; if !ta.is_invertible() { defmt::error!("Body velocity to wheel TA matrix is not invertible, cannot calculate the forward dynamics!") - } + } let t_wheel_to_body = ta.try_inverse().unwrap() * t_body_to_wheel.transpose(); RobotModel { @@ -52,4 +60,4 @@ impl RobotModel { pub fn wheel_vel_to_robot_vel(&self, wheel_vels: &Vector4) -> Vector3 { self.t_wheel_to_body * wheel_vels } -} \ No newline at end of file +} diff --git a/control-board/src/parameter_interface.rs b/control-board/src/parameter_interface.rs index 132ed867..8fbe1282 100644 --- a/control-board/src/parameter_interface.rs +++ b/control-board/src/parameter_interface.rs @@ -6,5 +6,8 @@ pub trait ParameterInterface { fn has_name(&self, param_name: ParameterName::Type) -> bool; - fn apply_command(&mut self, param_cmd: &ParameterCommand) -> Result; + fn apply_command( + &mut self, + param_cmd: &ParameterCommand, + ) -> Result; } diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 2be78c80..f55c4f5a 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -1,10 +1,14 @@ #![allow(dead_code)] -use ateam_common_packets::{bindings::{KickerTelemetry, PowerTelemetry}, radio::{DataPacket, TelemetryPacket}}; +use ateam_common_packets::{ + bindings::{KickerTelemetry, PowerTelemetry}, + radio::{DataPacket, TelemetryPacket}, +}; use embassy_stm32::{dma::NoDma, peripherals::*}; use embassy_sync::{ blocking_mutex::raw::ThreadModeRawMutex, - pubsub::{PubSubChannel, Publisher, Subscriber}}; + pubsub::{PubSubChannel, Publisher, Subscriber}, +}; use crate::tasks::dotstar_task::ControlBoardLedCommand; @@ -23,41 +27,114 @@ const LED_COMMAND_PUBSUB_DEPTH: usize = 5; type PubSubMutexType = ThreadModeRawMutex; pub type CommandsPubSub = PubSubChannel; -pub type CommandsPublisher = Publisher<'static, PubSubMutexType, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; -pub type CommandsSubscriber = Subscriber<'static, PubSubMutexType, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; - -pub type TelemetryPubSub = PubSubChannel; -pub type TelemetryPublisher = Publisher<'static, PubSubMutexType, TelemetryPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; -pub type TelemetrySubcriber = Subscriber<'static, PubSubMutexType, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; - -pub type GyroDataPubSub = PubSubChannel, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; -pub type GyroDataPublisher = Publisher<'static, PubSubMutexType, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; -pub type GyroDataSubscriber = Subscriber<'static, PubSubMutexType, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; -pub type AccelDataPubSub = PubSubChannel, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; -pub type AccelDataPublisher = Publisher<'static, PubSubMutexType, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; -pub type AccelDataSubscriber = Subscriber<'static, PubSubMutexType, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type CommandsPublisher = + Publisher<'static, PubSubMutexType, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; +pub type CommandsSubscriber = + Subscriber<'static, PubSubMutexType, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; + +pub type TelemetryPubSub = + PubSubChannel; +pub type TelemetryPublisher = + Publisher<'static, PubSubMutexType, TelemetryPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type TelemetrySubcriber = + Subscriber<'static, PubSubMutexType, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; + +pub type GyroDataPubSub = + PubSubChannel, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type GyroDataPublisher = + Publisher<'static, PubSubMutexType, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type GyroDataSubscriber = + Subscriber<'static, PubSubMutexType, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataPubSub = + PubSubChannel, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataPublisher = + Publisher<'static, PubSubMutexType, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataSubscriber = + Subscriber<'static, PubSubMutexType, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; const POWER_TELEM_PUBSUB_NUM_PUBS: usize = 1; const POWER_TELEM_PUBSUB_NUM_SUBS: usize = 1; -pub type PowerTelemetryPubSub = PubSubChannel; -pub type PowerTelemetryPublisher = Publisher<'static, PubSubMutexType, PowerTelemetry, POWER_TELEMETRY_PUBSUB_DEPTH, POWER_TELEM_PUBSUB_NUM_SUBS, POWER_TELEM_PUBSUB_NUM_PUBS>; -pub type PowerTelemetrySubscriber = Subscriber<'static, PubSubMutexType, PowerTelemetry, POWER_TELEMETRY_PUBSUB_DEPTH, POWER_TELEM_PUBSUB_NUM_SUBS, POWER_TELEM_PUBSUB_NUM_PUBS>; +pub type PowerTelemetryPubSub = PubSubChannel< + PubSubMutexType, + PowerTelemetry, + POWER_TELEMETRY_PUBSUB_DEPTH, + POWER_TELEM_PUBSUB_NUM_SUBS, + POWER_TELEM_PUBSUB_NUM_PUBS, +>; +pub type PowerTelemetryPublisher = Publisher< + 'static, + PubSubMutexType, + PowerTelemetry, + POWER_TELEMETRY_PUBSUB_DEPTH, + POWER_TELEM_PUBSUB_NUM_SUBS, + POWER_TELEM_PUBSUB_NUM_PUBS, +>; +pub type PowerTelemetrySubscriber = Subscriber< + 'static, + PubSubMutexType, + PowerTelemetry, + POWER_TELEMETRY_PUBSUB_DEPTH, + POWER_TELEM_PUBSUB_NUM_SUBS, + POWER_TELEM_PUBSUB_NUM_PUBS, +>; const KICKER_TELEM_PUBSUB_NUM_PUBS: usize = 1; const KICKER_TELEM_PUBSUB_NUM_SUBS: usize = 1; -pub type KickerTelemetryPubSub = PubSubChannel; -pub type KickerTelemetryPublisher = Publisher<'static, PubSubMutexType, KickerTelemetry, KICKER_TELEMETRY_PUBSUB_DEPTH, KICKER_TELEM_PUBSUB_NUM_SUBS, KICKER_TELEM_PUBSUB_NUM_PUBS>; -pub type KickerTelemetrySubscriber = Subscriber<'static, PubSubMutexType, KickerTelemetry, KICKER_TELEMETRY_PUBSUB_DEPTH, KICKER_TELEM_PUBSUB_NUM_SUBS, KICKER_TELEM_PUBSUB_NUM_PUBS>; +pub type KickerTelemetryPubSub = PubSubChannel< + PubSubMutexType, + KickerTelemetry, + KICKER_TELEMETRY_PUBSUB_DEPTH, + KICKER_TELEM_PUBSUB_NUM_SUBS, + KICKER_TELEM_PUBSUB_NUM_PUBS, +>; +pub type KickerTelemetryPublisher = Publisher< + 'static, + PubSubMutexType, + KickerTelemetry, + KICKER_TELEMETRY_PUBSUB_DEPTH, + KICKER_TELEM_PUBSUB_NUM_SUBS, + KICKER_TELEM_PUBSUB_NUM_PUBS, +>; +pub type KickerTelemetrySubscriber = Subscriber< + 'static, + PubSubMutexType, + KickerTelemetry, + KICKER_TELEMETRY_PUBSUB_DEPTH, + KICKER_TELEM_PUBSUB_NUM_SUBS, + KICKER_TELEM_PUBSUB_NUM_PUBS, +>; pub type BatteryVoltPubSub = PubSubChannel; -pub type BatteryVoltPublisher = Publisher<'static, PubSubMutexType, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; -pub type BatteryVoltSubscriber = Subscriber<'static, PubSubMutexType, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; +pub type BatteryVoltPublisher = + Publisher<'static, PubSubMutexType, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; +pub type BatteryVoltSubscriber = + Subscriber<'static, PubSubMutexType, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; const LED_COMMAND_PUBSUB_NUM_PUBS: usize = 3; const LED_COMMAND_PUBSUB_NUM_SUBS: usize = 1; -pub type LedCommandPubSub = PubSubChannel; -pub type LedCommandPublisher = Publisher<'static, PubSubMutexType, ControlBoardLedCommand, LED_COMMAND_PUBSUB_DEPTH, LED_COMMAND_PUBSUB_NUM_SUBS, LED_COMMAND_PUBSUB_NUM_PUBS>; -pub type LedCommandSubscriber = Subscriber<'static, PubSubMutexType, ControlBoardLedCommand, LED_COMMAND_PUBSUB_DEPTH, LED_COMMAND_PUBSUB_NUM_SUBS, LED_COMMAND_PUBSUB_NUM_PUBS>; +pub type LedCommandPubSub = PubSubChannel< + PubSubMutexType, + ControlBoardLedCommand, + LED_COMMAND_PUBSUB_DEPTH, + LED_COMMAND_PUBSUB_NUM_SUBS, + LED_COMMAND_PUBSUB_NUM_PUBS, +>; +pub type LedCommandPublisher = Publisher< + 'static, + PubSubMutexType, + ControlBoardLedCommand, + LED_COMMAND_PUBSUB_DEPTH, + LED_COMMAND_PUBSUB_NUM_SUBS, + LED_COMMAND_PUBSUB_NUM_PUBS, +>; +pub type LedCommandSubscriber = Subscriber< + 'static, + PubSubMutexType, + ControlBoardLedCommand, + LED_COMMAND_PUBSUB_DEPTH, + LED_COMMAND_PUBSUB_NUM_SUBS, + LED_COMMAND_PUBSUB_NUM_PUBS, +>; ///////////// // Radio // @@ -74,7 +151,6 @@ pub type RadioResetPin = PD7; pub type RadioBoot0Pin = PG9; pub type RadioNDetectPin = PA15; - ////////////// // Motors // ////////////// @@ -119,7 +195,6 @@ pub type MotorFRDmaRx = DMA1_CH7; pub type MotorFRBootPin = PB12; pub type MotorFRResetPin = PB13; - ////////////// // Kicker // ////////////// @@ -134,7 +209,6 @@ pub type KickerTxDma = DMA2_CH3; pub type KickerBootPin = PG2; pub type KickerResetPin = PG3; - /////////////////// // Power Board // /////////////////// @@ -147,7 +221,6 @@ pub type PowerUartTxPin = PG1; pub type PowerRxDma = DMA2_CH4; pub type PowerTxDma = DMA2_CH5; - //////////////////// // Optical Flow // //////////////////// @@ -162,7 +235,6 @@ pub type OpticalFlowDmaTx = NoDma; pub type OpticalFlowBootPin = PB7; pub type OpticalFlowResetPin = PB6; - ////////////////// // LCD Screen // ////////////////// @@ -174,7 +246,6 @@ pub type ScreenDmaRx = NoDma; pub type ScreenDmaTx = NoDma; pub type ScreenResetPin = PA8; - /////////// // USB // /////////// @@ -226,7 +297,6 @@ pub type VoltageMon3v3Pin = PA2; pub type BatteryAdc = ADC1; pub type VrefIntAdc = ADC3; - /////////////// // User IO // /////////////// @@ -281,4 +351,4 @@ pub type DotstarSpiMosi = PB5; pub type DotstarTxDma = BDMA_CH0; pub type BuzzerPin = PE6; -pub type BuzzerTimer = TIM15; // ch2 +pub type BuzzerTimer = TIM15; // ch2 diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index e975fee1..df151ccd 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -24,7 +24,6 @@ pub struct SharedRobotState { radio_bridge_ok: AtomicBool, radio_send_extended_telem: AtomicBool, - battery_low: AtomicBool, battery_crit: AtomicBool, @@ -49,10 +48,10 @@ impl SharedRobotState { imu_inop: AtomicBool::new(true), kicker_inop: AtomicBool::new(true), power_inop: AtomicBool::new(true), - wheels_inop: AtomicU8::new(0x0F), + wheels_inop: AtomicU8::new(0x0F), dribbler_inop: AtomicBool::new(true), last_packet_receive_time_ticks_ms: AtomicU32::new(0), - radio_network_ok: AtomicBool::new(false), + radio_network_ok: AtomicBool::new(false), radio_bridge_ok: AtomicBool::new(false), radio_send_extended_telem: AtomicBool::new(false), battery_low: AtomicBool::new(false), @@ -79,7 +78,7 @@ impl SharedRobotState { power_inop: self.get_power_inop(), wheels_inop: self.get_wheels_inop(), dribbler_inop: self.get_dribbler_inop(), - + last_packet_receive_time_ticks_ms: 0, radio_network_ok: self.get_radio_network_ok(), radio_bridge_ok: self.get_radio_bridge_ok(), @@ -101,7 +100,8 @@ impl SharedRobotState { } pub fn set_hw_init_state_valid(&self, hw_init_state_valid: bool) { - self.hw_init_state_valid.store(hw_init_state_valid, Ordering::Relaxed); + self.hw_init_state_valid + .store(hw_init_state_valid, Ordering::Relaxed); } pub fn get_hw_robot_id(&self) -> u8 { @@ -133,9 +133,10 @@ impl SharedRobotState { } pub fn set_hw_wifi_driver_use_flow_control(&self, use_flow_control: bool) { - self.hw_wifi_driver_use_flow_control.store(use_flow_control, Ordering::SeqCst); + self.hw_wifi_driver_use_flow_control + .store(use_flow_control, Ordering::SeqCst); } - + pub fn hw_in_debug_mode(&self) -> bool { self.hw_debug_mode.load(Ordering::Relaxed) } @@ -245,7 +246,8 @@ impl SharedRobotState { } pub fn set_radio_send_extended_telem(&self, send_extended_telem: bool) { - self.radio_send_extended_telem.store(send_extended_telem, Ordering::SeqCst); + self.radio_send_extended_telem + .store(send_extended_telem, Ordering::SeqCst); } pub fn ball_detected(&self) -> bool { @@ -261,7 +263,8 @@ impl SharedRobotState { } pub fn set_kicker_shutdown_complete(&self, shutdown_complete: bool) { - self.kicker_shutdown_complete.store(shutdown_complete, Ordering::Relaxed); + self.kicker_shutdown_complete + .store(shutdown_complete, Ordering::Relaxed); } } @@ -295,4 +298,4 @@ pub struct RobotState { pub ball_detected: bool, pub shutdown_requested: bool, pub kicker_shutdown_complete: bool, -} \ No newline at end of file +} diff --git a/control-board/src/songs.rs b/control-board/src/songs.rs index 283bbfe2..207cce24 100644 --- a/control-board/src/songs.rs +++ b/control-board/src/songs.rs @@ -1,56 +1,144 @@ use ateam_lib_stm32::audio::note::Beat; pub const TIPPED_WARNING_SONG: [Beat; 18] = [ - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 300_000 }, + Beat::Note { + tone: 392, + duration: 300_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 300_000 }, + Beat::Note { + tone: 392, + duration: 300_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 300_000 }, + Beat::Note { + tone: 392, + duration: 300_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { + tone: 392, + duration: 125_000, + }, Beat::Rest(1_000_000), ]; pub const BATTERY_WARNING_SONG: [Beat; 4] = [ - Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { + tone: 392, + duration: 250_000, + }, Beat::Rest(250_000), - Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { + tone: 392, + duration: 250_000, + }, Beat::Rest(250_000), ]; pub const BATTERY_CRITICAL_SONG: [Beat; 2] = [ - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 466, duration: 250_000 }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 466, + duration: 250_000, + }, ]; pub const TEST_SONG: [Beat; 17] = [ - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 523, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 523, duration: 250_000 }, - - Beat::Note { tone: 392, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 392, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 440, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 523, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - - Beat::Note { tone: 392, duration: 1_000_000 }, - ]; \ No newline at end of file + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 523, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 523, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 392, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 440, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 523, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 392, + duration: 1_000_000, + }, +]; diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index b70b9e97..30451071 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -1,6 +1,9 @@ -use core::{mem::MaybeUninit, f32::consts::PI}; +use core::{f32::consts::PI, mem::MaybeUninit}; -use ateam_lib_stm32::{drivers::boot::stm32_interface::Stm32Interface, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface::Stm32Interface, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; use defmt::*; use embassy_stm32::{ gpio::{Pin, Pull}, @@ -9,10 +12,15 @@ use embassy_stm32::{ use embassy_time::{with_timeout, Duration, Timer}; use nalgebra::Vector3; +use crate::{image_hash, DEBUG_MOTOR_UART_QUEUES}; use ateam_common_packets::bindings::{ - MotionCommandType::{self, OPEN_LOOP}, MotorCommandPacket, MotorCommandType::{MCP_MOTION, MCP_PARAMS}, MotorResponse, MotorResponseType::{MRP_MOTION, MRP_PARAMS}, MotorTelemetry, ParameterMotorResponse + MotionCommandType::{self, OPEN_LOOP}, + MotorCommandPacket, + MotorCommandType::{MCP_MOTION, MCP_PARAMS}, + MotorResponse, + MotorResponseType::{MRP_MOTION, MRP_PARAMS}, + MotorTelemetry, ParameterMotorResponse, }; -use crate::{image_hash, DEBUG_MOTOR_UART_QUEUES}; pub struct WheelMotor< 'a, @@ -21,14 +29,8 @@ pub struct WheelMotor< const DEPTH_RX: usize, const DEPTH_TX: usize, > { - stm32_uart_interface: Stm32Interface< - 'a, - LEN_RX, - LEN_TX, - DEPTH_RX, - DEPTH_TX, - DEBUG_MOTOR_UART_QUEUES, - >, + stm32_uart_interface: + Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_MOTOR_UART_QUEUES>, firmware_image: &'a [u8], current_state: MotorTelemetry, @@ -64,11 +66,10 @@ impl< LEN_TX, DEPTH_RX, DEPTH_TX, - DEBUG_MOTOR_UART_QUEUES + DEBUG_MOTOR_UART_QUEUES, >, firmware_image: &'a [u8], - ) -> WheelMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> - { + ) -> WheelMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { let start_state: MotorTelemetry = Default::default(); let start_params_state: ParameterMotorResponse = Default::default(); @@ -101,10 +102,17 @@ impl< boot0_pin: impl Pin, reset_pin: impl Pin, firmware_image: &'a [u8], - ) -> WheelMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> - { + ) -> WheelMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // Need a Pull None to allow for STSPIN watchdog usage. - let stm32_interface = Stm32Interface::new_from_pins(uart, read_queue, write_queue, boot0_pin, reset_pin, Pull::None, true); + let stm32_interface = Stm32Interface::new_from_pins( + uart, + read_queue, + write_queue, + boot0_pin, + reset_pin, + Pull::None, + true, + ); let start_state: MotorTelemetry = Default::default(); let start_params_state: ParameterMotorResponse = Default::default(); @@ -163,8 +171,11 @@ impl< if self.current_params_state.firmware_img_hash != [0; 4] { let current_img_hash = self.current_params_state.firmware_img_hash; defmt::debug!("Wheel Interface - Received parameter response"); - defmt::trace!("Wheel Interface - Current device image hash {:x}", current_img_hash); - return current_img_hash + defmt::trace!( + "Wheel Interface - Current device image hash {:x}", + current_img_hash + ); + return current_img_hash; }; Timer::after(Duration::from_millis(5)).await; @@ -173,10 +184,15 @@ impl< pub async fn check_device_has_latest_default_image(&mut self) -> Result { let latest_img_hash = self.get_latest_default_img_hash(); - defmt::debug!("Wheel Interface - Latest default image hash - {:x}", latest_img_hash); + defmt::debug!( + "Wheel Interface - Latest default image hash - {:x}", + latest_img_hash + ); defmt::trace!("Wheel Interface - Update UART config 2 MHz"); - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; let res; @@ -188,14 +204,16 @@ impl< defmt::trace!("Wheel Interface - Device has the latest default image"); res = Ok(true); } else { - defmt::trace!("Wheel Interface - Device does not have the latest default image"); + defmt::trace!( + "Wheel Interface - Device does not have the latest default image" + ); res = Ok(false); } - }, + } Err(_) => { defmt::debug!("Wheel Interface - No device response, image hash unknown"); res = Err(()); - }, + } } // Make sure that the uart queue is empty of any possible parameter // response packets, which may cause side effects for the flashing @@ -204,15 +222,23 @@ impl< return res; } - pub async fn init_firmware_image(&mut self, flash: bool, fw_image_bytes: &[u8]) -> Result<(), ()> { + pub async fn init_firmware_image( + &mut self, + flash: bool, + fw_image_bytes: &[u8], + ) -> Result<(), ()> { if flash { defmt::info!("Wheel Interface - Flashing firmware image"); - self.stm32_uart_interface.load_firmware_image(fw_image_bytes, true).await?; + self.stm32_uart_interface + .load_firmware_image(fw_image_bytes, true) + .await?; } else { defmt::info!("Wheel Interface - Skipping firmware flash"); } - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; @@ -238,7 +264,7 @@ impl< } else { flash = true; } - }, + } Err(_) => { flash = true; } @@ -252,7 +278,12 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("Drive Motor - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); + defmt::warn!( + "Drive Motor - Got invalid packet of len {:?} (expected {:?}) data: {:?}", + buf.len(), + core::mem::size_of::(), + buf + ); continue; } @@ -267,7 +298,6 @@ impl< *state.offset(i as isize) = buf[i]; } - // TODO probably do some checksum stuff eventually // decode union type, and reinterpret subtype @@ -279,7 +309,10 @@ impl< // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); if mrp.data.motion.master_error() != 0 { - error!("Drive Motor - Error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + error!( + "Drive Motor - Error: {:?}", + &mrp.data.motion._bitfield_1.get(0, 32) + ); } // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); @@ -352,7 +385,9 @@ impl< cmd.type_ = MCP_MOTION; cmd.crc32 = 0; cmd.data.motion.set_reset(self.reset_flagged as u32); - cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32); + cmd.data + .motion + .set_enable_telemetry(self.telemetry_enabled as u32); cmd.data.motion.motion_control_type = self.motion_type; cmd.data.motion.setpoint = self.setpoint; //info!("setpoint: {:?}", cmd.data.motion.setpoint); @@ -393,7 +428,9 @@ impl< } pub fn check_hall_error(&self) -> bool { - return self.current_state.hall_power_error() != 0 || self.current_state.hall_disconnected_error() != 0 || self.current_state.hall_enc_vel_disagreement_error() != 0; + return self.current_state.hall_power_error() != 0 + || self.current_state.hall_disconnected_error() != 0 + || self.current_state.hall_enc_vel_disagreement_error() != 0; } pub fn read_current(&self) -> f32 { diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index 6dfa039b..c158a069 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -1,15 +1,29 @@ use ateam_lib_stm32::{audio::tone_player::TonePlayer, drivers::audio::buzzer::Buzzer}; use embassy_executor::Spawner; -use embassy_stm32::{gpio::OutputType, time::hz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}}; +use embassy_stm32::{ + gpio::OutputType, + time::hz, + timer::{ + simple_pwm::{PwmPin, SimplePwm}, + Channel, + }, +}; use embassy_time::{Duration, Ticker}; -use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::TIPPED_WARNING_SONG}; +use crate::{ + pins::{BuzzerPin, BuzzerTimer}, + robot_state::SharedRobotState, + songs::TIPPED_WARNING_SONG, +}; #[macro_export] macro_rules! create_audio_task { ($main_spawner:ident, $robot_state:ident, $p:ident) => { ateam_control_board::tasks::audio_task::start_audio_task( - &$main_spawner, $robot_state, $p.TIM15, $p.PE6 + &$main_spawner, + $robot_state, + $p.TIM15, + $p.PE6, ); }; } @@ -24,7 +38,7 @@ async fn audio_task_entry( loop { let cur_robot_state = robot_state.get_state(); - // Structure so only one song can play per + // Structure so only one song can play per if cur_robot_state.battery_crit { // defmt::warn!("battery critical"); // let _ = tone_player.load_song(&BATTERY_CRITICAL_SONG); @@ -50,10 +64,20 @@ pub fn start_audio_task( buzzer_pin: BuzzerPin, ) { let ch2 = PwmPin::new_ch2(buzzer_pin, OutputType::PushPull); - let pwm = SimplePwm::new(buzzer_timer, None, Some(ch2), None, None, hz(1), Default::default()); - + let pwm = SimplePwm::new( + buzzer_timer, + None, + Some(ch2), + None, + None, + hz(1), + Default::default(), + ); + let audio_driver = Buzzer::new(pwm, Channel::Ch2); let tone_player = TonePlayer::new(audio_driver); - task_spawner.spawn(audio_task_entry(robot_state, tone_player)).unwrap(); -} \ No newline at end of file + task_spawner + .spawn(audio_task_entry(robot_state, tone_player)) + .unwrap(); +} diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index dc9ecae9..1798ec5d 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,13 +1,31 @@ -use ateam_common_packets::{bindings::{BasicControl, BasicTelemetry, KickerTelemetry, MotionCommandType, PowerTelemetry}, radio::TelemetryPacket}; -use ateam_lib_stm32::{drivers::boot::stm32_interface, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart}; +use ateam_common_packets::{ + bindings::{BasicControl, BasicTelemetry, KickerTelemetry, MotionCommandType, PowerTelemetry}, + radio::TelemetryPacket, +}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, +}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; use embassy_time::{Duration, Instant, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{self, params::robot_physical_params::{ - WHEEL_ANGLES_DEG, WHEEL_DISTANCE_TO_ROBOT_CENTER_M, WHEEL_RADIUS_M - }, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, parameter_interface::ParameterInterface, pins::*, robot_state::{RobotState, SharedRobotState}, stspin_motor::WheelMotor, SystemIrqs, DEBUG_MOTOR_UART_QUEUES, ROBOT_VERSION_MAJOR, ROBOT_VERSION_MINOR}; +use crate::{ + include_external_cpp_bin, + motion::{ + self, + params::robot_physical_params::{ + WHEEL_ANGLES_DEG, WHEEL_DISTANCE_TO_ROBOT_CENTER_M, WHEEL_RADIUS_M, + }, + robot_controller::BodyVelocityController, + robot_model::{RobotConstants, RobotModel}, + }, + parameter_interface::ParameterInterface, + pins::*, + robot_state::{RobotState, SharedRobotState}, + stspin_motor::WheelMotor, + SystemIrqs, DEBUG_MOTOR_UART_QUEUES, ROBOT_VERSION_MAJOR, ROBOT_VERSION_MINOR, +}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} @@ -16,7 +34,8 @@ const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 60; const RX_BUF_DEPTH: usize = 20; -type ControlWheelMotor = WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>; +type ControlWheelMotor = + WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>; static_idle_buffered_uart!(FRONT_LEFT, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_MOTOR_UART_QUEUES, #[link_section = ".axisram.buffers"]); static_idle_buffered_uart!(BACK_LEFT, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_MOTOR_UART_QUEUES, #[link_section = ".axisram.buffers"]); static_idle_buffered_uart!(BACK_RIGHT, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_MOTOR_UART_QUEUES, #[link_section = ".axisram.buffers"]); @@ -33,16 +52,45 @@ macro_rules! create_control_task { $p:ident ) => { ateam_control_board::tasks::control_task::start_control_task( - $main_spawner, $uart_queue_spawner, + $main_spawner, + $uart_queue_spawner, $robot_state, - $control_command_subscriber, $control_telemetry_publisher, - $control_gyro_data_subscriber, $control_accel_data_subscriber, - $power_telemetry_subscriber, $kicker_telemetry_subscriber, - $p.UART7, $p.PF6, $p.PF7, $p.DMA1_CH1, $p.DMA1_CH0, $p.PF5, $p.PF4, - $p.USART10, $p.PE2, $p.PE3, $p.DMA1_CH3, $p.DMA1_CH2, $p.PE5, $p.PE4, - $p.USART6, $p.PC7, $p.PC6, $p.DMA1_CH5, $p.DMA1_CH4, $p.PG7, $p.PG8, - $p.USART3, $p.PD9, $p.PD8, $p.DMA1_CH7, $p.DMA1_CH6, $p.PB12, $p.PB13, - ).await; + $control_command_subscriber, + $control_telemetry_publisher, + $control_gyro_data_subscriber, + $control_accel_data_subscriber, + $power_telemetry_subscriber, + $kicker_telemetry_subscriber, + $p.UART7, + $p.PF6, + $p.PF7, + $p.DMA1_CH1, + $p.DMA1_CH0, + $p.PF5, + $p.PF4, + $p.USART10, + $p.PE2, + $p.PE3, + $p.DMA1_CH3, + $p.DMA1_CH2, + $p.PE5, + $p.PE4, + $p.USART6, + $p.PC7, + $p.PC6, + $p.DMA1_CH5, + $p.DMA1_CH4, + $p.PG7, + $p.PG8, + $p.USART3, + $p.PD9, + $p.PD8, + $p.DMA1_CH7, + $p.DMA1_CH6, + $p.PB12, + $p.PB13, + ) + .await; }; } @@ -50,7 +98,8 @@ pub struct ControlTask< const MAX_RX_PACKET_SIZE: usize, const MAX_TX_PACKET_SIZE: usize, const RX_BUF_DEPTH: usize, - const TX_BUF_DEPTH: usize> { + const TX_BUF_DEPTH: usize, +> { shared_robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, @@ -72,449 +121,501 @@ pub struct ControlTask< motor_fr: ControlWheelMotor, } -impl < - const MAX_RX_PACKET_SIZE: usize, - const MAX_TX_PACKET_SIZE: usize, - const RX_BUF_DEPTH: usize, - const TX_BUF_DEPTH: usize> - ControlTask - { - - pub fn new(robot_state: &'static SharedRobotState, - command_subscriber: CommandsSubscriber, - telemetry_publisher: TelemetryPublisher, - gyro_subscriber: GyroDataSubscriber, - accel_subscriber: AccelDataSubscriber, - power_telemetry_subscriber: PowerTelemetrySubscriber, - kicker_telemetry_subscriber: KickerTelemetrySubscriber, - motor_fl: ControlWheelMotor, - motor_bl: ControlWheelMotor, - motor_br: ControlWheelMotor, - motor_fr: ControlWheelMotor, - ) -> Self { - ControlTask { - shared_robot_state: robot_state, - command_subscriber: command_subscriber, - telemetry_publisher: telemetry_publisher, - gyro_subscriber: gyro_subscriber, - accel_subscriber: accel_subscriber, - power_telemetry_subscriber, - kicker_telemetry_subscriber, - last_gyro_rads: 0.0, - last_accel_x_ms: 0.0, - last_accel_y_ms: 0.0, - last_command: Default::default(), - last_power_telemetry: Default::default(), - last_kicker_telemetry: Default::default(), - motor_fl: motor_fl, - motor_bl: motor_bl, - motor_br: motor_br, - motor_fr: motor_fr, - } +impl< + const MAX_RX_PACKET_SIZE: usize, + const MAX_TX_PACKET_SIZE: usize, + const RX_BUF_DEPTH: usize, + const TX_BUF_DEPTH: usize, + > ControlTask +{ + pub fn new( + robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + telemetry_publisher: TelemetryPublisher, + gyro_subscriber: GyroDataSubscriber, + accel_subscriber: AccelDataSubscriber, + power_telemetry_subscriber: PowerTelemetrySubscriber, + kicker_telemetry_subscriber: KickerTelemetrySubscriber, + motor_fl: ControlWheelMotor, + motor_bl: ControlWheelMotor, + motor_br: ControlWheelMotor, + motor_fr: ControlWheelMotor, + ) -> Self { + ControlTask { + shared_robot_state: robot_state, + command_subscriber: command_subscriber, + telemetry_publisher: telemetry_publisher, + gyro_subscriber: gyro_subscriber, + accel_subscriber: accel_subscriber, + power_telemetry_subscriber, + kicker_telemetry_subscriber, + last_gyro_rads: 0.0, + last_accel_x_ms: 0.0, + last_accel_y_ms: 0.0, + last_command: Default::default(), + last_power_telemetry: Default::default(), + last_kicker_telemetry: Default::default(), + motor_fl: motor_fl, + motor_bl: motor_bl, + motor_br: motor_br, + motor_fr: motor_fr, } + } - fn do_control_update(&mut self, - robot_controller: &mut BodyVelocityController, - cmd_vel: Vector3, - gyro_rads: f32, - controls_enabled: bool - ) -> Vector4 - /* + fn do_control_update( + &mut self, + robot_controller: &mut BodyVelocityController, + cmd_vel: Vector3, + gyro_rads: f32, + controls_enabled: bool, + ) -> Vector4 +/* Provide the motion controller with the current wheel velocities and torques from the appropriate sensors, then get a set of wheel velocities to apply based on the controller's current state. - */ - { - let wheel_vels = Vector4::new( - self.motor_fl.read_rads(), - self.motor_bl.read_rads(), - self.motor_br.read_rads(), - self.motor_fr.read_rads() - ); - - // torque values are computed on the spin but put in the current variable - // TODO update this when packet/var names are updated to match software - let wheel_torques = Vector4::new( - self.motor_fl.read_current(), - self.motor_bl.read_current(), - self.motor_br.read_current(), - self.motor_fr.read_current() - ); - - // TODO read from channel or something + */ { + let wheel_vels = Vector4::new( + self.motor_fl.read_rads(), + self.motor_bl.read_rads(), + self.motor_br.read_rads(), + self.motor_fr.read_rads(), + ); + + // torque values are computed on the spin but put in the current variable + // TODO update this when packet/var names are updated to match software + let wheel_torques = Vector4::new( + self.motor_fl.read_current(), + self.motor_bl.read_current(), + self.motor_br.read_current(), + self.motor_fr.read_current(), + ); + + // TODO read from channel or something + + robot_controller.control_update( + &cmd_vel, + &wheel_vels, + &wheel_torques, + gyro_rads, + controls_enabled, + ); + robot_controller.get_wheel_velocities() + } - robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); - robot_controller.get_wheel_velocities() + fn send_motor_commands_and_telemetry( + &mut self, + seq_number: u16, + robot_controller: &mut BodyVelocityController, + cur_state: RobotState, + ) { + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); + + let front_left_motor_error = self.motor_fl.read_is_error() as u32; + let back_left_motor_error = self.motor_bl.read_is_error() as u32; + let back_right_motor_error = self.motor_br.read_is_error() as u32; + let front_right_motor_error = self.motor_fr.read_is_error() as u32; + let dribbler_motor_error = self.last_kicker_telemetry.dribbler_motor.master_error() as u32; + + let front_left_hall_error = self.motor_fl.check_hall_error() as u32; + let back_left_hall_error = self.motor_bl.check_hall_error() as u32; + let back_right_hall_error = self.motor_br.check_hall_error() as u32; + let front_right_hall_error = self.motor_fr.check_hall_error() as u32; + let dribbler_motor_hall_error = self + .last_kicker_telemetry + .dribbler_motor + .hall_disconnected_error() as u32; + + let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + control_data_sequence_number: seq_number as u8, + transmission_sequence_number: 0, + robot_revision_major: ROBOT_VERSION_MAJOR, + robot_revision_minor: ROBOT_VERSION_MINOR, + _bitfield_align_1: Default::default(), + _bitfield_1: BasicTelemetry::new_bitfield_1( + !self.last_power_telemetry.power_ok() as u32, // power error + cur_state.power_inop as u32, // power board error + !self.last_power_telemetry.battery_info.battery_ok() as u32, // battery error + self.last_power_telemetry.battery_info.battery_low() as u32, // battery low + self.last_power_telemetry.battery_info.battery_critical() as u32, // battery crit + self.last_power_telemetry.shutdown_requested() as u32, // shutdown pending + cur_state.robot_tipped as u32, // tipped error + self.last_kicker_telemetry.error_detected() as u32, // breakbeam error + self.last_kicker_telemetry.ball_detected() as u32, // ball detected + cur_state.imu_inop as u32, // accel 0 error + false as u32, // accel 1 error, uninstalled + cur_state.imu_inop as u32, // gyro 0 error + false as u32, // gyro 1 error, uninstalled + front_left_motor_error, + front_left_hall_error, + back_left_motor_error, + back_left_hall_error, + back_right_motor_error, + back_right_hall_error, + front_right_motor_error, + front_right_hall_error, + dribbler_motor_error, + dribbler_motor_hall_error, + self.last_kicker_telemetry.error_detected() as u32, + false as u32, // chipper available + (!cur_state.kicker_inop && self.last_kicker_telemetry.error_detected() == 0) as u32, + self.last_command.body_vel_controls_enabled(), + self.last_command.wheel_vel_control_enabled(), + self.last_command.wheel_torque_control_enabled(), + Default::default(), + ), + battery_percent: self.last_power_telemetry.battery_info.battery_pct as u16, + kicker_charge_percent: self.last_kicker_telemetry.charge_pct, + }); + + if cur_state.radio_bridge_ok { + self.telemetry_publisher.publish_immediate(basic_telem); } - fn send_motor_commands_and_telemetry(&mut self, - seq_number: u16, - robot_controller: &mut BodyVelocityController, - cur_state: RobotState) - { - - - self.motor_fl.send_motion_command(); - self.motor_bl.send_motion_command(); - self.motor_br.send_motion_command(); - self.motor_fr.send_motion_command(); - - let front_left_motor_error = self.motor_fl.read_is_error() as u32; - let back_left_motor_error = self.motor_bl.read_is_error() as u32; - let back_right_motor_error = self.motor_br.read_is_error() as u32; - let front_right_motor_error = self.motor_fr.read_is_error() as u32; - let dribbler_motor_error = self.last_kicker_telemetry.dribbler_motor.master_error() as u32; - - let front_left_hall_error = self.motor_fl.check_hall_error() as u32; - let back_left_hall_error = self.motor_bl.check_hall_error() as u32; - let back_right_hall_error = self.motor_br.check_hall_error() as u32; - let front_right_hall_error = self.motor_fr.check_hall_error() as u32; - let dribbler_motor_hall_error = self.last_kicker_telemetry.dribbler_motor.hall_disconnected_error() as u32; - - let basic_telem = TelemetryPacket::Basic(BasicTelemetry { - control_data_sequence_number: seq_number as u8, - transmission_sequence_number: 0, - robot_revision_major: ROBOT_VERSION_MAJOR, - robot_revision_minor: ROBOT_VERSION_MINOR, - _bitfield_align_1: Default::default(), - _bitfield_1: BasicTelemetry::new_bitfield_1( - !self.last_power_telemetry.power_ok() as u32, // power error - cur_state.power_inop as u32, // power board error - !self.last_power_telemetry.battery_info.battery_ok() as u32, // battery error - self.last_power_telemetry.battery_info.battery_low() as u32, // battery low - self.last_power_telemetry.battery_info.battery_critical() as u32, // battery crit - self.last_power_telemetry.shutdown_requested() as u32, // shutdown pending - cur_state.robot_tipped as u32, // tipped error - self.last_kicker_telemetry.error_detected() as u32, // breakbeam error - self.last_kicker_telemetry.ball_detected() as u32, // ball detected - cur_state.imu_inop as u32, // accel 0 error - false as u32, // accel 1 error, uninstalled - cur_state.imu_inop as u32, // gyro 0 error - false as u32, // gyro 1 error, uninstalled - front_left_motor_error, - front_left_hall_error, - back_left_motor_error, - back_left_hall_error, - back_right_motor_error, - back_right_hall_error, - front_right_motor_error, - front_right_hall_error, - dribbler_motor_error, - dribbler_motor_hall_error, - self.last_kicker_telemetry.error_detected() as u32, - false as u32, // chipper available - (!cur_state.kicker_inop && self.last_kicker_telemetry.error_detected() == 0) as u32, - self.last_command.body_vel_controls_enabled(), - self.last_command.wheel_vel_control_enabled(), - self.last_command.wheel_torque_control_enabled(), - Default::default()), - battery_percent: self.last_power_telemetry.battery_info.battery_pct as u16, - kicker_charge_percent: self.last_kicker_telemetry.charge_pct, - }); - - if cur_state.radio_bridge_ok { - self.telemetry_publisher.publish_immediate(basic_telem); - } - - let mut control_debug_telem = robot_controller.get_control_debug_telem(); + let mut control_debug_telem = robot_controller.get_control_debug_telem(); - control_debug_telem.front_left_motor = self.motor_fl.get_latest_state(); - control_debug_telem.back_left_motor = self.motor_bl.get_latest_state(); - control_debug_telem.back_right_motor = self.motor_br.get_latest_state(); - control_debug_telem.front_right_motor = self.motor_fr.get_latest_state(); + control_debug_telem.front_left_motor = self.motor_fl.get_latest_state(); + control_debug_telem.back_left_motor = self.motor_bl.get_latest_state(); + control_debug_telem.back_right_motor = self.motor_br.get_latest_state(); + control_debug_telem.front_right_motor = self.motor_fr.get_latest_state(); - control_debug_telem.imu_accel[0] = self.last_accel_x_ms; - control_debug_telem.imu_accel[1] = self.last_accel_y_ms; + control_debug_telem.imu_accel[0] = self.last_accel_x_ms; + control_debug_telem.imu_accel[1] = self.last_accel_y_ms; - control_debug_telem.kicker_status = self.last_kicker_telemetry; - control_debug_telem.power_status = self.last_power_telemetry; + control_debug_telem.kicker_status = self.last_kicker_telemetry; + control_debug_telem.power_status = self.last_power_telemetry; - let control_debug_telem = TelemetryPacket::Extended(control_debug_telem); - if cur_state.radio_bridge_ok { - self.telemetry_publisher.publish_immediate(control_debug_telem); - } + let control_debug_telem = TelemetryPacket::Extended(control_debug_telem); + if cur_state.radio_bridge_ok { + self.telemetry_publisher + .publish_immediate(control_debug_telem); } + } - async fn control_task_entry(&mut self) { - defmt::info!("control task init."); - - // wait for the switch state to be read - while !self.shared_robot_state.hw_init_state_valid() { - Timer::after_millis(10).await; - } - - self.flash_motor_firmware( - self.shared_robot_state.hw_in_debug_mode()).await; - - embassy_futures::join::join4( - self.motor_fl.leave_reset(), - self.motor_bl.leave_reset(), - self.motor_br.leave_reset(), - self.motor_fr.leave_reset(), - ).await; - - - self.motor_fl.set_telemetry_enabled(true); - self.motor_bl.set_telemetry_enabled(true); - self.motor_br.set_telemetry_enabled(true); - self.motor_fr.set_telemetry_enabled(true); + async fn control_task_entry(&mut self) { + defmt::info!("control task init."); + // wait for the switch state to be read + while !self.shared_robot_state.hw_init_state_valid() { Timer::after_millis(10).await; + } - let robot_model = self.get_robot_model(); - let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); - - let mut ctrl_seq_number = 0; - let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); - let mut ticks_since_control_packet = 0; - - let mut last_loop_term_time = Instant::now(); - - loop { - let loop_start_time = Instant::now(); - let loop_invocation_dead_time = loop_start_time - last_loop_term_time; - if loop_start_time - last_loop_term_time > Duration::from_millis(11) { - defmt::warn!("control loop scheuling lagged. Expected ~10ms between loop invocations, but got {:?}us", loop_invocation_dead_time.as_micros()); - } - - self.motor_fl.process_packets(); - self.motor_bl.process_packets(); - self.motor_br.process_packets(); - self.motor_fr.process_packets(); + self.flash_motor_firmware(self.shared_robot_state.hw_in_debug_mode()) + .await; - let cur_state = self.shared_robot_state.get_state(); + embassy_futures::join::join4( + self.motor_fl.leave_reset(), + self.motor_bl.leave_reset(), + self.motor_br.leave_reset(), + self.motor_fr.leave_reset(), + ) + .await; - // self.motor_fl.log_reset("FL"); - // self.motor_bl.log_reset("BL"); - // self.motor_br.log_reset("BR"); - // self.motor_fr.log_reset("FR"); + self.motor_fl.set_telemetry_enabled(true); + self.motor_bl.set_telemetry_enabled(true); + self.motor_br.set_telemetry_enabled(true); + self.motor_fr.set_telemetry_enabled(true); + Timer::after_millis(10).await; - ticks_since_control_packet += 1; - while let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { - match latest_packet { - ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + let robot_model = self.get_robot_model(); + let mut robot_controller = + BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); - let new_cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); + let mut ctrl_seq_number = 0; + let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - cmd_vel = new_cmd_vel; - ticks_since_control_packet = 0; + let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); + let mut ticks_since_control_packet = 0; - if latest_control.reboot_robot() != 0 { - loop { - cortex_m::peripheral::SCB::sys_reset(); - } - } + let mut last_loop_term_time = Instant::now(); - if latest_control.request_shutdown() != 0 { - self.shared_robot_state.flag_shutdown_requested(); - } + loop { + let loop_start_time = Instant::now(); + let loop_invocation_dead_time = loop_start_time - last_loop_term_time; + if loop_start_time - last_loop_term_time > Duration::from_millis(11) { + defmt::warn!("control loop scheuling lagged. Expected ~10ms between loop invocations, but got {:?}us", loop_invocation_dead_time.as_micros()); + } - let wheel_motion_type = match (self.last_command.wheel_vel_control_enabled() != 0, self.last_command.wheel_torque_control_enabled() != 0) { - (true, true) => MotionCommandType::BOTH, - (true, false) => MotionCommandType::VELOCITY, - (false, true) => MotionCommandType::TORQUE, - (false, false) => MotionCommandType::OPEN_LOOP, - }; - - self.motor_fl.set_motion_type(wheel_motion_type); - self.motor_bl.set_motion_type(wheel_motion_type); - self.motor_br.set_motion_type(wheel_motion_type); - self.motor_fr.set_motion_type(wheel_motion_type); - - self.last_command = latest_control; - }, - ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { - let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); - - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); - self.telemetry_publisher.publish(tp_resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); - self.telemetry_publisher.publish(tp_resp).await; + self.motor_fl.process_packets(); + self.motor_bl.process_packets(); + self.motor_br.process_packets(); + self.motor_fr.process_packets(); + + let cur_state = self.shared_robot_state.get_state(); + + // self.motor_fl.log_reset("FL"); + // self.motor_bl.log_reset("BL"); + // self.motor_br.log_reset("BR"); + // self.motor_fr.log_reset("FR"); + + ticks_since_control_packet += 1; + while let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { + match latest_packet { + ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + let new_cmd_vel = Vector3::new( + latest_control.vel_x_linear, + latest_control.vel_y_linear, + latest_control.vel_z_angular, + ); + + cmd_vel = new_cmd_vel; + ticks_since_control_packet = 0; + + if latest_control.reboot_robot() != 0 { + loop { + cortex_m::peripheral::SCB::sys_reset(); } - }, + } + + if latest_control.request_shutdown() != 0 { + self.shared_robot_state.flag_shutdown_requested(); + } + + let wheel_motion_type = match ( + self.last_command.wheel_vel_control_enabled() != 0, + self.last_command.wheel_torque_control_enabled() != 0, + ) { + (true, true) => MotionCommandType::BOTH, + (true, false) => MotionCommandType::VELOCITY, + (false, true) => MotionCommandType::TORQUE, + (false, false) => MotionCommandType::OPEN_LOOP, + }; + + self.motor_fl.set_motion_type(wheel_motion_type); + self.motor_bl.set_motion_type(wheel_motion_type); + self.motor_br.set_motion_type(wheel_motion_type); + self.motor_fr.set_motion_type(wheel_motion_type); + + self.last_command = latest_control; + } + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { + let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); + + if let Ok(resp) = param_cmd_resp { + defmt::info!("sending successful parameter update command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } else if let Err(resp) = param_cmd_resp { + defmt::warn!("sending failed parameter updated command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } } } + } - if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { - cmd_vel = Vector3::new(0., 0., 0.); - //defmt::warn!("ticks since packet lockout"); - } - - // now we have setpoint r(t) in self.cmd_vel - - while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { - self.last_gyro_rads = gyro_rads[2]; - } - - while let Some(accel_ms) = self.accel_subscriber.try_next_message_pure() { - self.last_accel_x_ms = accel_ms[0]; - self.last_accel_y_ms = accel_ms[1]; - } + if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + //defmt::warn!("ticks since packet lockout"); + } - while let Some(kicker_telemetry) = self.kicker_telemetry_subscriber.try_next_message_pure() { - self.last_kicker_telemetry = kicker_telemetry; - } + // now we have setpoint r(t) in self.cmd_vel - while let Some(power_telemetry) = self.power_telemetry_subscriber.try_next_message_pure() { - self.last_power_telemetry = power_telemetry; - } + while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { + self.last_gyro_rads = gyro_rads[2]; + } - if self.stop_wheels() { - cmd_vel = Vector3::new(0.0, 0.0, 0.0); - } else if self.last_command.game_state_in_stop() != 0 { - // TODO impl 1.5m/s clamping or something - } + while let Some(accel_ms) = self.accel_subscriber.try_next_message_pure() { + self.last_accel_x_ms = accel_ms[0]; + self.last_accel_y_ms = accel_ms[1]; + } - let wheel_vels = if self.stop_wheels() { - defmt::warn!("control task - motor commands locked out"); - Vector4::new(0.0, 0.0, 0.0, 0.0) - } else { - let controls_enabled = self.last_command.body_vel_controls_enabled() != 0; - self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_rads, controls_enabled) - }; + while let Some(kicker_telemetry) = + self.kicker_telemetry_subscriber.try_next_message_pure() + { + self.last_kicker_telemetry = kicker_telemetry; + } - self.motor_fl.set_setpoint(wheel_vels[0]); - self.motor_bl.set_setpoint(wheel_vels[1]); - self.motor_br.set_setpoint(wheel_vels[2]); - self.motor_fr.set_setpoint(wheel_vels[3]); + while let Some(power_telemetry) = + self.power_telemetry_subscriber.try_next_message_pure() + { + self.last_power_telemetry = power_telemetry; + } - // 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()); + if self.stop_wheels() { + cmd_vel = Vector3::new(0.0, 0.0, 0.0); + } else if self.last_command.game_state_in_stop() != 0 { + // TODO impl 1.5m/s clamping or something + } + let wheel_vels = if self.stop_wheels() { + defmt::warn!("control task - motor commands locked out"); + Vector4::new(0.0, 0.0, 0.0, 0.0) + } else { + let controls_enabled = self.last_command.body_vel_controls_enabled() != 0; + self.do_control_update( + &mut robot_controller, + cmd_vel, + self.last_gyro_rads, + controls_enabled, + ) + }; - /////////////////////////////////// - // send commands and telemetry // - /////////////////////////////////// + self.motor_fl.set_setpoint(wheel_vels[0]); + self.motor_bl.set_setpoint(wheel_vels[1]); + self.motor_br.set_setpoint(wheel_vels[2]); + self.motor_fr.set_setpoint(wheel_vels[3]); - self.send_motor_commands_and_telemetry(ctrl_seq_number, - &mut robot_controller, cur_state); + // 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()); - // increment seq number - ctrl_seq_number = (ctrl_seq_number + 1) & 0x00FF; + /////////////////////////////////// + // send commands and telemetry // + /////////////////////////////////// - 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!("control 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); - } + self.send_motor_commands_and_telemetry( + ctrl_seq_number, + &mut robot_controller, + cur_state, + ); - last_loop_term_time = Instant::now(); + // increment seq number + ctrl_seq_number = (ctrl_seq_number + 1) & 0x00FF; - loop_rate_ticker.next().await; + 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!("control 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); } - } - async fn flash_motor_firmware(&mut self, debug: bool) { - defmt::info!("flashing firmware"); - let force_flash = debug; - if debug { - let mut had_motor_error = false; - if self.motor_fl.init_default_firmware_image(force_flash).await.is_err() { - defmt::error!("failed to flash FL"); - had_motor_error = true; - } else { - defmt::info!("FL flashed"); - } + last_loop_term_time = Instant::now(); - if self.motor_bl.init_default_firmware_image(force_flash).await.is_err() { - defmt::error!("failed to flash BL"); - had_motor_error = true; - } else { - defmt::info!("BL flashed"); - } - - if self.motor_br.init_default_firmware_image(force_flash).await.is_err() { - defmt::error!("failed to flash BR"); - had_motor_error = true; - } else { - defmt::info!("BR flashed"); - } + loop_rate_ticker.next().await; + } + } - if self.motor_fr.init_default_firmware_image(force_flash).await.is_err() { - defmt::error!("failed to flash FR"); - had_motor_error = true; - } else { - defmt::info!("FR flashed"); - } + async fn flash_motor_firmware(&mut self, debug: bool) { + defmt::info!("flashing firmware"); + let force_flash = debug; + if debug { + let mut had_motor_error = false; + if self + .motor_fl + .init_default_firmware_image(force_flash) + .await + .is_err() + { + defmt::error!("failed to flash FL"); + had_motor_error = true; + } else { + defmt::info!("FL flashed"); + } - if had_motor_error { - defmt::error!("one or more motors failed to flash.") - } else { - defmt::debug!("all motors flashed"); - } + if self + .motor_bl + .init_default_firmware_image(force_flash) + .await + .is_err() + { + defmt::error!("failed to flash BL"); + had_motor_error = true; } else { - let res = embassy_futures::join::join4( - self.motor_fl.init_default_firmware_image(force_flash), - self.motor_bl.init_default_firmware_image(force_flash), - self.motor_br.init_default_firmware_image(force_flash), - self.motor_fr.init_default_firmware_image(force_flash), - ) - .await; + defmt::info!("BL flashed"); + } - let error_mask = res.0.is_err() as u8 - | ((res.1.is_err() as u8) & 0x01) << 1 - | ((res.2.is_err() as u8) & 0x01) << 2 - | ((res.3.is_err() as u8) & 0x01) << 3; + if self + .motor_br + .init_default_firmware_image(force_flash) + .await + .is_err() + { + defmt::error!("failed to flash BR"); + had_motor_error = true; + } else { + defmt::info!("BR flashed"); + } - self.shared_robot_state.set_wheels_inop(error_mask); + if self + .motor_fr + .init_default_firmware_image(force_flash) + .await + .is_err() + { + defmt::error!("failed to flash FR"); + had_motor_error = true; + } else { + defmt::info!("FR flashed"); + } - if error_mask != 0 { - defmt::error!("failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", res); - } else { - defmt::debug!("motor firmware flashed"); - } + if had_motor_error { + defmt::error!("one or more motors failed to flash.") + } else { + defmt::debug!("all motors flashed"); + } + } else { + let res = embassy_futures::join::join4( + self.motor_fl.init_default_firmware_image(force_flash), + self.motor_bl.init_default_firmware_image(force_flash), + self.motor_br.init_default_firmware_image(force_flash), + self.motor_fr.init_default_firmware_image(force_flash), + ) + .await; + + let error_mask = res.0.is_err() as u8 + | ((res.1.is_err() as u8) & 0x01) << 1 + | ((res.2.is_err() as u8) & 0x01) << 2 + | ((res.3.is_err() as u8) & 0x01) << 3; + + self.shared_robot_state.set_wheels_inop(error_mask); + + if error_mask != 0 { + defmt::error!( + "failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", + res + ); + } else { + defmt::debug!("motor firmware flashed"); } } + } - fn get_robot_model(&mut self) -> motion::robot_model::RobotModel{ - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - return robot_model; - } + fn get_robot_model(&mut self) -> motion::robot_model::RobotModel { + let robot_model_constants: RobotConstants = RobotConstants { + wheel_angles_rad: Vector4::new( + WHEEL_ANGLES_DEG[0].to_radians(), + WHEEL_ANGLES_DEG[1].to_radians(), + WHEEL_ANGLES_DEG[2].to_radians(), + WHEEL_ANGLES_DEG[3].to_radians(), + ), + wheel_radius_m: Vector4::new( + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + ), + wheel_dist_to_cent_m: Vector4::new( + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + ), + }; + + let robot_model: RobotModel = RobotModel::new(robot_model_constants); + + return robot_model; + } - fn stop_wheels(&self) -> bool { - // defmt::debug!("hco: {}, sd req: {}, estop: {}", self.last_power_telemetry.high_current_operations_allowed() == 0, self.shared_robot_state.shutdown_requested(), self.last_command.emergency_stop() != 0); + fn stop_wheels(&self) -> bool { + // defmt::debug!("hco: {}, sd req: {}, estop: {}", self.last_power_telemetry.high_current_operations_allowed() == 0, self.shared_robot_state.shutdown_requested(), self.last_command.emergency_stop() != 0); - // self.last_power_telemetry.high_current_operations_allowed() == 0 - self.shared_robot_state.shutdown_requested() - || self.last_command.emergency_stop() != 0 - } + // self.last_power_telemetry.high_current_operations_allowed() == 0 + self.shared_robot_state.shutdown_requested() || self.last_command.emergency_stop() != 0 } +} #[embassy_executor::task] -async fn control_task_entry(mut control_task: ControlTask) { +async fn control_task_entry( + mut control_task: ControlTask< + MAX_RX_PACKET_SIZE, + MAX_TX_PACKET_SIZE, + RX_BUF_DEPTH, + TX_BUF_DEPTH, + >, +) { loop { control_task.control_task_entry().await; defmt::error!("control task returned"); @@ -531,11 +632,34 @@ pub async fn start_control_task( accel_subscriber: AccelDataSubscriber, power_telemetry_subscriber: PowerTelemetrySubscriber, kicker_telemetry_subscriber: KickerTelemetrySubscriber, - motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, - motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin, - motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin, - motor_fr_uart: MotorFRUart, motor_fr_rx_pin: MotorFRUartRxPin, motor_fr_tx_pin: MotorFRUartTxPin, motor_fr_rx_dma: MotorFRDmaRx, motor_fr_tx_dma: MotorFRDmaTx, motor_fr_boot0_pin: MotorFRBootPin, motor_fr_nrst_pin: MotorFRResetPin, - + motor_fl_uart: MotorFLUart, + motor_fl_rx_pin: MotorFLUartRxPin, + motor_fl_tx_pin: MotorFLUartTxPin, + motor_fl_rx_dma: MotorFLDmaRx, + motor_fl_tx_dma: MotorFLDmaTx, + motor_fl_boot0_pin: MotorFLBootPin, + motor_fl_nrst_pin: MotorFLResetPin, + motor_bl_uart: MotorBLUart, + motor_bl_rx_pin: MotorBLUartRxPin, + motor_bl_tx_pin: MotorBLUartTxPin, + motor_bl_rx_dma: MotorBLDmaRx, + motor_bl_tx_dma: MotorBLDmaTx, + motor_bl_boot0_pin: MotorBLBootPin, + motor_bl_nrst_pin: MotorBLResetPin, + motor_br_uart: MotorBRUart, + motor_br_rx_pin: MotorBRUartRxPin, + motor_br_tx_pin: MotorBRUartTxPin, + motor_br_rx_dma: MotorBRDmaRx, + motor_br_tx_dma: MotorBRDmaTx, + motor_br_boot0_pin: MotorBRBootPin, + motor_br_nrst_pin: MotorBRResetPin, + motor_fr_uart: MotorFRUart, + motor_fr_rx_pin: MotorFRUartRxPin, + motor_fr_tx_pin: MotorFRUartTxPin, + motor_fr_rx_dma: MotorFRDmaRx, + motor_fr_tx_dma: MotorFRDmaTx, + motor_fr_boot0_pin: MotorFRBootPin, + motor_fr_nrst_pin: MotorFRResetPin, ) { let initial_motor_controller_uart_conifg = stm32_interface::get_bootloader_uart_config(); @@ -543,10 +667,46 @@ pub async fn start_control_task( // create motor uarts // ////////////////////////// - let fl_uart = Uart::new(motor_fl_uart, motor_fl_rx_pin, motor_fl_tx_pin, SystemIrqs, motor_fl_tx_dma, motor_fl_rx_dma, initial_motor_controller_uart_conifg).unwrap(); - let bl_uart = Uart::new(motor_bl_uart, motor_bl_rx_pin, motor_bl_tx_pin, SystemIrqs, motor_bl_tx_dma, motor_bl_rx_dma, initial_motor_controller_uart_conifg).unwrap(); - let br_uart = Uart::new(motor_br_uart, motor_br_rx_pin, motor_br_tx_pin, SystemIrqs, motor_br_tx_dma, motor_br_rx_dma, initial_motor_controller_uart_conifg).unwrap(); - let fr_uart = Uart::new(motor_fr_uart, motor_fr_rx_pin, motor_fr_tx_pin, SystemIrqs, motor_fr_tx_dma, motor_fr_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + let fl_uart = Uart::new( + motor_fl_uart, + motor_fl_rx_pin, + motor_fl_tx_pin, + SystemIrqs, + motor_fl_tx_dma, + motor_fl_rx_dma, + initial_motor_controller_uart_conifg, + ) + .unwrap(); + let bl_uart = Uart::new( + motor_bl_uart, + motor_bl_rx_pin, + motor_bl_tx_pin, + SystemIrqs, + motor_bl_tx_dma, + motor_bl_rx_dma, + initial_motor_controller_uart_conifg, + ) + .unwrap(); + let br_uart = Uart::new( + motor_br_uart, + motor_br_rx_pin, + motor_br_tx_pin, + SystemIrqs, + motor_br_tx_dma, + motor_br_rx_dma, + initial_motor_controller_uart_conifg, + ) + .unwrap(); + let fr_uart = Uart::new( + motor_fr_uart, + motor_fr_rx_pin, + motor_fr_tx_pin, + SystemIrqs, + motor_fr_tx_dma, + motor_fr_rx_dma, + initial_motor_controller_uart_conifg, + ) + .unwrap(); ////////////////////////////////////////////// // register motor queues and DMA hardware // @@ -566,16 +726,54 @@ pub async fn start_control_task( // create motor controllers // //////////////////////////////// - let motor_fl = WheelMotor::new_from_pins(&FRONT_LEFT_IDLE_BUFFERED_UART, FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_fl_boot0_pin, motor_fl_nrst_pin, WHEEL_FW_IMG); - let motor_bl = WheelMotor::new_from_pins(&BACK_LEFT_IDLE_BUFFERED_UART, BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_bl_boot0_pin, motor_bl_nrst_pin, WHEEL_FW_IMG); - let motor_br = WheelMotor::new_from_pins(&BACK_RIGHT_IDLE_BUFFERED_UART, BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_br_boot0_pin, motor_br_nrst_pin, WHEEL_FW_IMG); - let motor_fr = WheelMotor::new_from_pins(&FRONT_RIGHT_IDLE_BUFFERED_UART, FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_fr_boot0_pin, motor_fr_nrst_pin, WHEEL_FW_IMG); + let motor_fl = WheelMotor::new_from_pins( + &FRONT_LEFT_IDLE_BUFFERED_UART, + FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), + FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), + motor_fl_boot0_pin, + motor_fl_nrst_pin, + WHEEL_FW_IMG, + ); + let motor_bl = WheelMotor::new_from_pins( + &BACK_LEFT_IDLE_BUFFERED_UART, + BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), + BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), + motor_bl_boot0_pin, + motor_bl_nrst_pin, + WHEEL_FW_IMG, + ); + let motor_br = WheelMotor::new_from_pins( + &BACK_RIGHT_IDLE_BUFFERED_UART, + BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), + BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), + motor_br_boot0_pin, + motor_br_nrst_pin, + WHEEL_FW_IMG, + ); + let motor_fr = WheelMotor::new_from_pins( + &FRONT_RIGHT_IDLE_BUFFERED_UART, + FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), + FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), + motor_fr_boot0_pin, + motor_fr_nrst_pin, + WHEEL_FW_IMG, + ); let control_task = ControlTask::new( - robot_state, command_subscriber, telemetry_publisher, - gyro_subscriber, accel_subscriber, - power_telemetry_subscriber, kicker_telemetry_subscriber, - motor_fl, motor_bl, motor_br, motor_fr); - - control_task_spawner.spawn(control_task_entry(control_task)).unwrap(); -} \ No newline at end of file + robot_state, + command_subscriber, + telemetry_publisher, + gyro_subscriber, + accel_subscriber, + power_telemetry_subscriber, + kicker_telemetry_subscriber, + motor_fl, + motor_bl, + motor_br, + motor_fr, + ); + + control_task_spawner + .spawn(control_task_entry(control_task)) + .unwrap(); +} diff --git a/control-board/src/tasks/dotstar_task.rs b/control-board/src/tasks/dotstar_task.rs index fea19356..b29f685c 100644 --- a/control-board/src/tasks/dotstar_task.rs +++ b/control-board/src/tasks/dotstar_task.rs @@ -30,7 +30,7 @@ pub enum RadioStatusLedCommand { ConnectedSoftware, Ok, Error, -} +} #[derive(Debug, Clone, Copy, defmt::Format)] pub enum KickerStatusLedCommand { @@ -60,9 +60,15 @@ pub enum ControlBoardLedCommand { #[macro_export] macro_rules! create_dotstar_task { ($spawner:ident, $led_command_subscriber:ident, $p:ident) => { - ateam_control_board::tasks::dotstar_task::start_dotstar_task(&$spawner, + ateam_control_board::tasks::dotstar_task::start_dotstar_task( + &$spawner, $led_command_subscriber, - $p.SPI6, $p.PB3, $p.PB5, $p.BDMA_CH0).await; + $p.SPI6, + $p.PB3, + $p.PB5, + $p.BDMA_CH0, + ) + .await; }; } @@ -77,7 +83,7 @@ async fn dotstar_task_entry( dotstars.set_drv_str_all(32); dotstars.set_color_all(BLACK); dotstars.update().await; - + loop { let led_command = led_command_subscriber.next_message_pure().await; match led_command { @@ -98,40 +104,75 @@ async fn dotstar_task_entry( }; dotstars.set_color(led_color, led_index as usize); - }, - ControlBoardLedCommand::Imu(imu_status_led_command) => { - match imu_status_led_command { - ImuStatusLedCommand::Configuring => dotstars.set_color(PURPLE, ControlDotstarIndex::Imu.into()), - ImuStatusLedCommand::Ok=> dotstars.set_color(GREEN, ControlDotstarIndex::Imu.into()), - ImuStatusLedCommand::Error => dotstars.set_color(RED, ControlDotstarIndex::Imu.into()), + } + ControlBoardLedCommand::Imu(imu_status_led_command) => match imu_status_led_command { + ImuStatusLedCommand::Configuring => { + dotstars.set_color(PURPLE, ControlDotstarIndex::Imu.into()) + } + ImuStatusLedCommand::Ok => { + dotstars.set_color(GREEN, ControlDotstarIndex::Imu.into()) + } + ImuStatusLedCommand::Error => { + dotstars.set_color(RED, ControlDotstarIndex::Imu.into()) } }, ControlBoardLedCommand::Radio(radio_status_led_command) => { match radio_status_led_command { - RadioStatusLedCommand::Off => dotstars.set_color(BLACK, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::ConnectedPhys => dotstars.set_color(CYAN, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::ConnectedUart => dotstars.set_color(BLUE, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::ConnectedNetwork => dotstars.set_color(VIOLET, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::ConnectedSoftware => dotstars.set_color(GREEN, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::Ok=> dotstars.set_color(GREEN, ControlDotstarIndex::Radio.into()), - RadioStatusLedCommand::Error => dotstars.set_color(RED, ControlDotstarIndex::Radio.into()) + RadioStatusLedCommand::Off => { + dotstars.set_color(BLACK, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::ConnectedPhys => { + dotstars.set_color(CYAN, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::ConnectedUart => { + dotstars.set_color(BLUE, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::ConnectedNetwork => { + dotstars.set_color(VIOLET, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::ConnectedSoftware => { + dotstars.set_color(GREEN, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::Ok => { + dotstars.set_color(GREEN, ControlDotstarIndex::Radio.into()) + } + RadioStatusLedCommand::Error => { + dotstars.set_color(RED, ControlDotstarIndex::Radio.into()) + } } - }, + } ControlBoardLedCommand::Kicker(kicker_status_led_command) => { match kicker_status_led_command { - KickerStatusLedCommand::Ok => dotstars.set_color(GREEN, ControlDotstarIndex::Kicker.into()), - KickerStatusLedCommand::Error => dotstars.set_color(RED, ControlDotstarIndex::Kicker.into()), } - }, + KickerStatusLedCommand::Ok => { + dotstars.set_color(GREEN, ControlDotstarIndex::Kicker.into()) + } + KickerStatusLedCommand::Error => { + dotstars.set_color(RED, ControlDotstarIndex::Kicker.into()) + } + } + } ControlBoardLedCommand::General(control_general_led_command) => { match control_general_led_command { - ControlGeneralLedCommand::ShutdownRequested => dotstars.set_color(ORANGE_RED, ControlDotstarIndex::User2.into()), - ControlGeneralLedCommand::BallDetected => dotstars.set_color(BLUE, ControlDotstarIndex::User2.into()), - ControlGeneralLedCommand::BallNotDetected => dotstars.set_color(BLACK, ControlDotstarIndex::User2.into()), - ControlGeneralLedCommand::Ok => dotstars.set_color(GREEN, ControlDotstarIndex::User1.into()), - ControlGeneralLedCommand::Warn => dotstars.set_color(YELLOW, ControlDotstarIndex::User1.into()), - ControlGeneralLedCommand::Error => dotstars.set_color(RED, ControlDotstarIndex::User1.into()), + ControlGeneralLedCommand::ShutdownRequested => { + dotstars.set_color(ORANGE_RED, ControlDotstarIndex::User2.into()) + } + ControlGeneralLedCommand::BallDetected => { + dotstars.set_color(BLUE, ControlDotstarIndex::User2.into()) + } + ControlGeneralLedCommand::BallNotDetected => { + dotstars.set_color(BLACK, ControlDotstarIndex::User2.into()) + } + ControlGeneralLedCommand::Ok => { + dotstars.set_color(GREEN, ControlDotstarIndex::User1.into()) + } + ControlGeneralLedCommand::Warn => { + dotstars.set_color(YELLOW, ControlDotstarIndex::User1.into()) + } + ControlGeneralLedCommand::Error => { + dotstars.set_color(RED, ControlDotstarIndex::User1.into()) + } } - }, + } } dotstars.update().await; @@ -165,16 +206,25 @@ pub type Apa102Buf = [u8; LED_BUF_LEN]; #[link_section = ".sram4"] static mut DOTSTAR_SPI_BUFFER_CELL: Apa102Buf = [0; LED_BUF_LEN]; -pub async fn start_dotstar_task(spawner: &Spawner, +pub async fn start_dotstar_task( + spawner: &Spawner, led_command_subscriber: LedCommandSubscriber, dotstar_peri: DotstarSpi, dotstar_sck_pin: DotstarSpiSck, dotstar_mosi_pin: DotstarSpiMosi, dotstar_tx_dma: DotstarTxDma, - ) { - - let dotstar_spi_buf: &'static mut Apa102Buf = unsafe { &mut *(&raw mut DOTSTAR_SPI_BUFFER_CELL) }; - let dotstars = Apa102::::new_from_pins(dotstar_peri, dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, dotstar_spi_buf.into()); - - spawner.spawn(dotstar_task_entry(led_command_subscriber, dotstars)).unwrap(); -} \ No newline at end of file +) { + let dotstar_spi_buf: &'static mut Apa102Buf = + unsafe { &mut *(&raw mut DOTSTAR_SPI_BUFFER_CELL) }; + let dotstars = Apa102::::new_from_pins( + dotstar_peri, + dotstar_sck_pin, + dotstar_mosi_pin, + dotstar_tx_dma, + dotstar_spi_buf.into(), + ); + + spawner + .spawn(dotstar_task_entry(led_command_subscriber, dotstars)) + .unwrap(); +} diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index d57461e3..fbd9adb0 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -1,9 +1,9 @@ use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; -use embassy_stm32::Peripheral; use embassy_stm32::exti::ExtiInput; use embassy_stm32::gpio::Pull; -use embassy_stm32::spi::{SckPin, MisoPin, MosiPin}; +use embassy_stm32::spi::{MisoPin, MosiPin, SckPin}; +use embassy_stm32::Peripheral; use embassy_time::{Instant, Timer}; use nalgebra::Vector3; @@ -19,52 +19,80 @@ const TIPPED_MIN_DURATION_MS: u64 = 1000; #[macro_export] macro_rules! create_imu_task { ($main_spawner:ident, $robot_state:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $imu_led_cmd_pub:ident, $p:ident) => { - ateam_control_board::tasks::imu_task::start_imu_task(&$main_spawner, + ateam_control_board::tasks::imu_task::start_imu_task( + &$main_spawner, $robot_state, - $imu_gyro_data_publisher, $imu_accel_data_publisher, $imu_led_cmd_pub, - $p.SPI1, $p.PA5, $p.PA7, $p.PA6, - $p.DMA2_CH7, $p.DMA2_CH6, - $p.PA4, $p.PA3, $p.PC4, - $p.PB0, $p.PB1, $p.EXTI0, $p.EXTI1, - $p.PB2); + $imu_gyro_data_publisher, + $imu_accel_data_publisher, + $imu_led_cmd_pub, + $p.SPI1, + $p.PA5, + $p.PA7, + $p.PA6, + $p.DMA2_CH7, + $p.DMA2_CH6, + $p.PA4, + $p.PA3, + $p.PC4, + $p.PB0, + $p.PB1, + $p.EXTI0, + $p.EXTI1, + $p.PB2, + ); }; } #[macro_export] macro_rules! create_imu_task_ie { ($main_spawner:ident, $robot_state:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $imu_led_cmd_pub:ident, $p:ident) => { - ateam_control_board::tasks::imu_task::start_imu_task_ie(&$main_spawner, + ateam_control_board::tasks::imu_task::start_imu_task_ie( + &$main_spawner, $robot_state, - $imu_gyro_data_publisher, $imu_accel_data_publisher, $imu_led_cmd_pub, - $p.SPI1, $p.PA5, $p.PA7, $p.PA6, - $p.DMA2_CH7, $p.DMA2_CH6, - $p.PA4, $p.PA3, $p.PC4, - $p.PB0, $p.PB1, $p.EXTI0, $p.EXTI1, - $p.PB2); + $imu_gyro_data_publisher, + $imu_accel_data_publisher, + $imu_led_cmd_pub, + $p.SPI1, + $p.PA5, + $p.PA7, + $p.PA6, + $p.DMA2_CH7, + $p.DMA2_CH6, + $p.PA4, + $p.PA3, + $p.PC4, + $p.PB0, + $p.PB1, + $p.EXTI0, + $p.EXTI1, + $p.PB2, + ); }; } #[link_section = ".axisram.buffers"] static mut IMU_BUFFER_CELL: [u8; bmi323::SPI_MIN_BUF_LEN] = [0; bmi323::SPI_MIN_BUF_LEN]; - #[embassy_executor::task] async fn imu_task_entry( - robot_state: &'static SharedRobotState, - gyro_pub: GyroDataPublisher, - accel_pub: AccelDataPublisher, - led_command_pub: LedCommandPublisher, - mut imu: Bmi323<'static, 'static>, - mut _accel_int: ExtiInput<'static>, - mut gyro_int: ExtiInput<'static>) { - + robot_state: &'static SharedRobotState, + gyro_pub: GyroDataPublisher, + accel_pub: AccelDataPublisher, + led_command_pub: LedCommandPublisher, + mut imu: Bmi323<'static, 'static>, + mut _accel_int: ExtiInput<'static>, + mut gyro_int: ExtiInput<'static>, +) { defmt::info!("imu start startup."); - let mut first_tipped_check_time = Instant::now(); + let mut first_tipped_check_time = Instant::now(); let mut first_tipped_seen = false; - 'imu_configuration_loop: - loop { - led_command_pub.publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Configuring)).await; + 'imu_configuration_loop: loop { + led_command_pub + .publish(ControlBoardLedCommand::Imu( + ImuStatusLedCommand::Configuring, + )) + .await; // At the beginning, assume IMU is not working yet. robot_state.set_imu_inop(true); @@ -72,48 +100,67 @@ async fn imu_task_entry( let self_test_res = imu.self_test().await; if self_test_res.is_err() { defmt::error!("IMU self test failed"); - led_command_pub.publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)).await; + led_command_pub + .publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)) + .await; Timer::after_millis(1000).await; continue 'imu_configuration_loop; } - + // configure the gyro, map int to int pin 2 - let gyro_config_res = imu.set_gyro_config(GyroMode::ContinuousHighPerformance, - GyroRange::PlusMinus2000DegPerSec, - Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr100p0, - DataAveragingWindow::Average2Samples).await; - imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2).await; + let gyro_config_res = imu + .set_gyro_config( + GyroMode::ContinuousHighPerformance, + GyroRange::PlusMinus2000DegPerSec, + Bandwidth3DbCutoffFreq::AccOdrOver2, + OutputDataRate::Odr100p0, + DataAveragingWindow::Average2Samples, + ) + .await; + imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2) + .await; if gyro_config_res.is_err() { - led_command_pub.publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)).await; + led_command_pub + .publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)) + .await; defmt::error!("gyro configration failed."); } - + // configure the gyro, map int to int pin 1 - let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance, - AccelRange::Range2g, - Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr100p0, - DataAveragingWindow::Average2Samples).await; - imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1).await; + let acc_config_res = imu + .set_accel_config( + AccelMode::ContinuousHighPerformance, + AccelRange::Range2g, + Bandwidth3DbCutoffFreq::AccOdrOver2, + OutputDataRate::Odr100p0, + DataAveragingWindow::Average2Samples, + ) + .await; + imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1) + .await; if acc_config_res.is_err() { - led_command_pub.publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)).await; + led_command_pub + .publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Error)) + .await; defmt::error!("accel configration failed."); } // configure the phys properties of the int pins - imu.set_int1_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull).await; - imu.set_int2_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull).await; + imu.set_int1_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull) + .await; + imu.set_int2_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull) + .await; // enable gyro int imu.set_int2_enabled(true).await; - led_command_pub.publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Ok)).await; + led_command_pub + .publish(ControlBoardLedCommand::Imu(ImuStatusLedCommand::Ok)) + .await; - 'imu_data_loop: - loop { + 'imu_data_loop: loop { // block on gyro interrupt, active low match select(gyro_int.wait_for_falling_edge(), Timer::after_millis(1000)).await { Either::First(_) => { @@ -123,11 +170,15 @@ async fn imu_task_entry( // read gyro data let imu_data = imu.gyro_get_data_rads().await; gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2])); - + // read accel data // TODO: don't use raw data, impl conversion let accel_data = imu.accel_get_data_mps().await; - accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); + accel_pub.publish_immediate(Vector3::new( + accel_data[0] as f32, + accel_data[1] as f32, + accel_data[2] as f32, + )); // TODO: magic number, fix after raw data conversion if accel_data[2] < 4.0 { @@ -138,7 +189,11 @@ 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().as_millis() > TIPPED_MIN_DURATION_MS { + if Instant::checked_duration_since(&cur_time, first_tipped_check_time) + .unwrap() + .as_millis() + > TIPPED_MIN_DURATION_MS + { robot_state.set_robot_tipped(true); } else { // If it hasn't been long enough, clear the robot tipped. @@ -146,7 +201,7 @@ async fn imu_task_entry( } } } else { - // Not tipped so clear everything. + // Not tipped so clear everything. first_tipped_seen = false; robot_state.set_robot_tipped(false); } @@ -154,40 +209,39 @@ async fn imu_task_entry( Either::Second(_) => { defmt::warn!("imu interrupt based data acq timed out."); // attempt connect validation and reconfig - break 'imu_data_loop + break 'imu_data_loop; } }; } } - - } pub fn start_imu_task( - imu_task_spawner: &Spawner, - robot_state: &'static SharedRobotState, - gyro_data_publisher: GyroDataPublisher, - accel_data_publisher: AccelDataPublisher, - led_cmd_publisher: LedCommandPublisher, - peri: impl Peripheral

+ 'static, - sck: impl Peripheral

> + 'static, - mosi: impl Peripheral

> + 'static, - miso: impl Peripheral

> + 'static, - txdma: impl Peripheral

+ 'static, - rxdma: impl Peripheral

+ 'static, - bmi323_nss: ImuSpiNss0Pin, - _ext_nss1_pin: ExtImuSpiNss1Pin, - _ext_nss2_pin: ExtImuSpiNss2Pin, - accel_int_pin: impl Peripheral

+ 'static, - gyro_int_pin: impl Peripheral

+ 'static, - accel_int: impl Peripheral

::ExtiChannel> + 'static, - gyro_int: impl Peripheral

::ExtiChannel> + 'static, - _ext_imu_det_pin: ExtImuNDetPin) { + imu_task_spawner: &Spawner, + robot_state: &'static SharedRobotState, + gyro_data_publisher: GyroDataPublisher, + accel_data_publisher: AccelDataPublisher, + led_cmd_publisher: LedCommandPublisher, + peri: impl Peripheral

+ 'static, + sck: impl Peripheral

> + 'static, + mosi: impl Peripheral

> + 'static, + miso: impl Peripheral

> + 'static, + txdma: impl Peripheral

+ 'static, + rxdma: impl Peripheral

+ 'static, + bmi323_nss: ImuSpiNss0Pin, + _ext_nss1_pin: ExtImuSpiNss1Pin, + _ext_nss2_pin: ExtImuSpiNss2Pin, + accel_int_pin: impl Peripheral

+ 'static, + gyro_int_pin: impl Peripheral

+ 'static, + accel_int: impl Peripheral

::ExtiChannel> + 'static, + gyro_int: impl Peripheral

::ExtiChannel> + 'static, + _ext_imu_det_pin: ExtImuNDetPin, +) { defmt::debug!("starting imu task..."); // let imu_buf = IMU_BUFFER_CELL.take(); // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; - let imu_buf: & mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; + let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); @@ -196,7 +250,17 @@ pub fn start_imu_task( let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::None); let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::None); - imu_task_spawner.spawn(imu_task_entry(robot_state, gyro_data_publisher, accel_data_publisher, led_cmd_publisher, imu, accel_int, gyro_int)).unwrap(); + imu_task_spawner + .spawn(imu_task_entry( + robot_state, + gyro_data_publisher, + accel_data_publisher, + led_cmd_publisher, + imu, + accel_int, + gyro_int, + )) + .unwrap(); } pub fn start_imu_task_via_ie( @@ -218,12 +282,13 @@ pub fn start_imu_task_via_ie( gyro_int_pin: impl Peripheral

+ 'static, accel_int: impl Peripheral

::ExtiChannel> + 'static, gyro_int: impl Peripheral

::ExtiChannel> + 'static, - _ext_imu_det_pin: ExtImuNDetPin) { + _ext_imu_det_pin: ExtImuNDetPin, +) { defmt::debug!("starting imu task..."); // let imu_buf = IMU_BUFFER_CELL.take(); // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; - let imu_buf: & mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; + let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); @@ -232,6 +297,15 @@ pub fn start_imu_task_via_ie( let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::None); let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::None); - imu_task_spawner.spawn(imu_task_entry(robot_state, gyro_data_publisher, accel_data_publisher, led_cmd_publisher, imu, accel_int, gyro_int)).unwrap(); + imu_task_spawner + .spawn(imu_task_entry( + robot_state, + gyro_data_publisher, + accel_data_publisher, + led_cmd_publisher, + imu, + accel_int, + gyro_int, + )) + .unwrap(); } - diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 3d3c6c43..ea9f495c 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -1,11 +1,18 @@ use ateam_common_packets::{bindings::KickRequest, radio::DataPacket}; -use ateam_lib_stm32::{drivers::boot::stm32_interface, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface, + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::{gpio::Pin, usart::Uart}; use embassy_sync::pubsub::WaitResult; -use embassy_time::{Duration, Ticker, Instant}; +use embassy_time::{Duration, Instant, Ticker}; -use crate::{drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, SystemIrqs, DEBUG_KICKER_UART_QUEUES}; +use crate::{ + drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, + SystemIrqs, DEBUG_KICKER_UART_QUEUES, +}; include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} @@ -22,13 +29,20 @@ macro_rules! create_kicker_task { ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, $command_subscriber:ident, $kicker_telemetry_publisher:ident, $p:ident) => { ateam_control_board::tasks::kicker_task::start_kicker_task( - $main_spawner, $uart_queue_spawner, + $main_spawner, + $uart_queue_spawner, $robot_state, $command_subscriber, $kicker_telemetry_publisher, - $p.UART8, $p.PE0, $p.PE1, - $p.DMA2_CH2, $p.DMA2_CH3, - $p.PG2, $p.PG3).await; + $p.UART8, + $p.PE0, + $p.PE1, + $p.DMA2_CH2, + $p.DMA2_CH3, + $p.PG2, + $p.PG3, + ) + .await; }; } @@ -44,11 +58,13 @@ enum KickerTaskState { WaitShutdown, } -pub struct KickerTask<'a, +pub struct KickerTask< + 'a, const LEN_RX: usize, const LEN_TX: usize, const DEPTH_RX: usize, - const DEPTH_TX: usize> { + const DEPTH_TX: usize, +> { kicker_driver: Kicker<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX>, kicker_task_state: KickerTaskState, robot_state: &'static SharedRobotState, @@ -57,15 +73,20 @@ pub struct KickerTask<'a, kicker_telemetry_publisher: KickerTelemetryPublisher, } -impl<'a, -const LEN_RX: usize, -const LEN_TX: usize, -const DEPTH_RX: usize, -const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - pub fn new(kicker_driver: Kicker<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX>, - robot_state: &'static SharedRobotState, - command_subscriber: CommandsSubscriber, - kicker_telemetry_publisher: KickerTelemetryPublisher) -> Self { +impl< + 'a, + const LEN_RX: usize, + const LEN_TX: usize, + const DEPTH_RX: usize, + const DEPTH_TX: usize, + > KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> +{ + pub fn new( + kicker_driver: Kicker<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX>, + robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + kicker_telemetry_publisher: KickerTelemetryPublisher, + ) -> Self { KickerTask { kicker_driver: kicker_driver, kicker_task_state: KickerTaskState::PoweredOff, @@ -85,17 +106,29 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { firmware_image: &'a [u8], robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, - kicker_telemetry_publisher: KickerTelemetryPublisher) -> Self { - - let kicker_driver = Kicker::new_from_pins(uart, read_queue, write_queue, boot0_pin, reset_pin, firmware_image); - - Self::new(kicker_driver, robot_state, command_subscriber, kicker_telemetry_publisher) + kicker_telemetry_publisher: KickerTelemetryPublisher, + ) -> Self { + let kicker_driver = Kicker::new_from_pins( + uart, + read_queue, + write_queue, + boot0_pin, + reset_pin, + firmware_image, + ); + + Self::new( + kicker_driver, + robot_state, + command_subscriber, + kicker_telemetry_publisher, + ) } pub async fn kicker_task_entry(&mut self) { let mut main_loop_ticker = Ticker::every(Duration::from_hz(100)); // Connection timeout start will be reset when entering ConnectUart state and when a telemetry packet is received - let mut connection_timeout_start = Instant::now(); // This initial value shouldn't ever be used to calculate a timeout + let mut connection_timeout_start = Instant::now(); // This initial value shouldn't ever be used to calculate a timeout loop { let cur_robot_state = self.robot_state.get_state(); @@ -103,12 +136,16 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { let telemetry_received = self.kicker_driver.process_telemetry(); if telemetry_received { // Publish telemetry - self.kicker_telemetry_publisher.publish_immediate(self.kicker_driver.get_lastest_state()); + self.kicker_telemetry_publisher + .publish_immediate(self.kicker_driver.get_lastest_state()); // Reset the connection timeout period connection_timeout_start = Instant::now(); } 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(); + let time_elapsed = + Instant::checked_duration_since(&Instant::now(), connection_timeout_start) + .unwrap() + .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; @@ -132,25 +169,31 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { KickerTaskState::PoweredOff => { if cur_robot_state.hw_init_state_valid && !cur_robot_state.shutdown_requested { self.kicker_task_state = KickerTaskState::PowerOn; - } + } // Should we hold the kicker in reset here? - // else + // else // { // // Hold kicker in reset until control board is ready // self.kicker_driver.enter_reset().await; // } - }, + } KickerTaskState::PowerOn => { // lets power settle on kicker - defmt::trace!("Kicker Interface - State '{}' - Resetting kicker", self.kicker_task_state); + defmt::trace!( + "Kicker Interface - State '{}' - Resetting kicker", + self.kicker_task_state + ); self.kicker_driver.reset().await; self.kicker_task_state = KickerTaskState::InitFirmware; - }, + } KickerTaskState::InitFirmware => { // Ensure firmware image is up to date let force_flash = cur_robot_state.hw_debug_mode; // This await can be over 100 ms to check if kicker responds with current image hash - let res = self.kicker_driver.init_default_firmware_image(force_flash).await; + let res = self + .kicker_driver + .init_default_firmware_image(force_flash) + .await; if res.is_ok() { // Firmware is up to date, move on to Reset state self.kicker_task_state = KickerTaskState::Reset; @@ -166,49 +209,66 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // Rate limit the retry loop // Timer::after_millis(10).await; } - }, + } KickerTaskState::Reset => { - defmt::trace!("Kicker Interface - State '{}' - Resetting kicker", self.kicker_task_state); + defmt::trace!( + "Kicker Interface - State '{}' - Resetting kicker", + self.kicker_task_state + ); self.kicker_driver.reset().await; // Reset the connection timeout period connection_timeout_start = Instant::now(); // Move on to ConnectUart state self.kicker_task_state = KickerTaskState::ConnectUart; - }, + } KickerTaskState::ConnectUart => { if telemetry_received { - defmt::info!("Kicker Interface - State '{}' - Kicker UART connection established", self.kicker_task_state); + defmt::info!( + "Kicker Interface - State '{}' - Kicker UART connection established", + self.kicker_task_state + ); // Move on to Connected state self.kicker_task_state = KickerTaskState::Connected; } - }, + } KickerTaskState::Connected => { self.connected_poll_loop().await; // external events will move us out of this state - }, + } KickerTaskState::InitiateShutdown => { - defmt::trace!("Kicker Interface - State '{}' - Requesting shutdown", self.kicker_task_state); + defmt::trace!( + "Kicker Interface - State '{}' - Requesting shutdown", + self.kicker_task_state + ); self.kicker_driver.request_shutdown(); // wait for kicker to ack shutdown if self.kicker_driver.shutdown_acknowledge() { - defmt::info!("Kicker Interface - State '{}' - Kicker acknowledged shutdown request", self.kicker_task_state); + defmt::info!( + "Kicker Interface - State '{}' - Kicker acknowledged shutdown request", + self.kicker_task_state + ); self.kicker_task_state = KickerTaskState::WaitShutdown; } - }, + } KickerTaskState::WaitShutdown => { if self.kicker_driver.shutdown_completed() { - defmt::info!("Kicker Interface - State '{}' - Kicker finished shutdown", self.kicker_task_state); + defmt::info!( + "Kicker Interface - State '{}' - Kicker finished shutdown", + self.kicker_task_state + ); self.kicker_task_state = KickerTaskState::PoweredOff; self.robot_state.set_kicker_shutdown_complete(true); } - }, + } } // kicker and radio loop rates are both 100Hz, so 10ms packet interval // if we miss 10 in a row, something has gone quite wrong // override commands to safe ones - if Instant::now() - self.last_command_received_time.unwrap_or(Instant::now()) > Duration::from_millis(500) { + if Instant::now() - self.last_command_received_time.unwrap_or(Instant::now()) + > Duration::from_millis(500) + { // Avoid spamming logs while the system starts up defmt::error!("Kicker Interface - Kicker task has stopped receiving commands from the radio task and will de-arm the kicker board"); self.kicker_driver.set_kick_strength(0.0); @@ -229,7 +289,8 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { self.robot_state.set_ball_detected(ball_detected); } - self.robot_state.set_kicker_inop(self.kicker_task_state < KickerTaskState::Connected); + self.robot_state + .set_kicker_inop(self.kicker_task_state < KickerTaskState::Connected); main_loop_ticker.next().await; } @@ -242,9 +303,12 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { match pkt { WaitResult::Lagged(amnt) => { if amnt > 3 { - defmt::warn!("Kicker Interface - Kicker task lagged processing commands by {} msgs", amnt); + defmt::warn!( + "Kicker Interface - Kicker task lagged processing commands by {} msgs", + amnt + ); } - }, + } WaitResult::Message(cmd) => { match cmd { DataPacket::BasicControl(bc_pkt) => { @@ -253,27 +317,37 @@ const DEPTH_TX: usize> KickerTask<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { self.kicker_driver.set_kick_strength(bc_pkt.kick_vel); self.kicker_driver.request_kick(bc_pkt.kick_request); self.kicker_driver.set_drib_vel(bc_pkt.dribbler_speed); - self.kicker_driver.set_drib_multiplier(bc_pkt.dribbler_multiplier()); - }, + self.kicker_driver + .set_drib_multiplier(bc_pkt.dribbler_multiplier()); + } DataPacket::ParameterCommand(_) => { // we currently don't have any kicker parameters - }, + } } - }, + } } } } } #[embassy_executor::task] -async fn kicker_task_entry(mut kicker_task: KickerTask<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>) { +async fn kicker_task_entry( + mut kicker_task: KickerTask< + 'static, + MAX_RX_PACKET_SIZE, + MAX_TX_PACKET_SIZE, + RX_BUF_DEPTH, + TX_BUF_DEPTH, + >, +) { loop { kicker_task.kicker_task_entry().await; defmt::error!("Kicker Interface - Kicker task returned"); } } -pub async fn start_kicker_task(kicker_task_spawner: Spawner, +pub async fn start_kicker_task( + kicker_task_spawner: Spawner, queue_spawner: SendSpawner, robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, @@ -284,15 +358,36 @@ pub async fn start_kicker_task(kicker_task_spawner: Spawner, kicker_uart_rx_dma: KickerRxDma, kicker_uart_tx_dma: KickerTxDma, kicker_boot0_pin: KickerBootPin, - kicker_reset_pin: KickerResetPin) { - + kicker_reset_pin: KickerResetPin, +) { let initial_kicker_uart_conifg = stm32_interface::get_bootloader_uart_config(); - let kicker_uart = Uart::new(kicker_uart, kicker_uart_rx_pin, kicker_uart_tx_pin, SystemIrqs, kicker_uart_tx_dma, kicker_uart_rx_dma, initial_kicker_uart_conifg).unwrap(); + let kicker_uart = Uart::new( + kicker_uart, + kicker_uart_rx_pin, + kicker_uart_tx_pin, + SystemIrqs, + kicker_uart_tx_dma, + kicker_uart_rx_dma, + initial_kicker_uart_conifg, + ) + .unwrap(); KICKER_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(queue_spawner, KICKER, kicker_uart); - let kicker_task = KickerTask::new_from_pins(&KICKER_IDLE_BUFFERED_UART, KICKER_IDLE_BUFFERED_UART.get_uart_read_queue(), KICKER_IDLE_BUFFERED_UART.get_uart_write_queue(), kicker_boot0_pin, kicker_reset_pin, KICKER_FW_IMG, robot_state, command_subscriber, kicker_telemetry_publisher); - kicker_task_spawner.spawn(kicker_task_entry(kicker_task)).unwrap(); + let kicker_task = KickerTask::new_from_pins( + &KICKER_IDLE_BUFFERED_UART, + KICKER_IDLE_BUFFERED_UART.get_uart_read_queue(), + KICKER_IDLE_BUFFERED_UART.get_uart_write_queue(), + kicker_boot0_pin, + kicker_reset_pin, + KICKER_FW_IMG, + robot_state, + command_subscriber, + kicker_telemetry_publisher, + ); + kicker_task_spawner + .spawn(kicker_task_entry(kicker_task)) + .unwrap(); } diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index 40cb7e45..607ff73f 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -3,6 +3,6 @@ pub mod control_task; pub mod dotstar_task; pub mod imu_task; pub mod kicker_task; -pub mod radio_task; pub mod power_task; -pub mod user_io_task; \ No newline at end of file +pub mod radio_task; +pub mod user_io_task; diff --git a/control-board/src/tasks/power_task.rs b/control-board/src/tasks/power_task.rs index c8d511b9..7e49ff5f 100644 --- a/control-board/src/tasks/power_task.rs +++ b/control-board/src/tasks/power_task.rs @@ -1,20 +1,36 @@ use core::mem::MaybeUninit; use ateam_common_packets::bindings::{PowerCommand, PowerTelemetry}; -use ateam_lib_stm32::{idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::{self, DataBits, Parity, StopBits, Uart}; use embassy_time::{Duration, Instant, Ticker, Timer}; -use crate::{pins::*, robot_state::SharedRobotState, tasks::dotstar_task::{ControlBoardLedCommand, ControlGeneralLedCommand}, SystemIrqs, DEBUG_POWER_UART_QUEUES}; +use crate::{ + pins::*, + robot_state::SharedRobotState, + tasks::dotstar_task::{ControlBoardLedCommand, ControlGeneralLedCommand}, + SystemIrqs, DEBUG_POWER_UART_QUEUES, +}; #[macro_export] macro_rules! create_power_task { ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, $power_telemetry_publisher:ident, $led_cmd_pub:ident, $p:ident) => { ateam_control_board::tasks::power_task::start_power_task( - $main_spawner, $uart_queue_spawner, - $robot_state, $power_telemetry_publisher, $led_cmd_pub, - $p.UART9, $p.PG0, $p.PG1, $p.DMA2_CH4, $p.DMA2_CH5); + $main_spawner, + $uart_queue_spawner, + $robot_state, + $power_telemetry_publisher, + $led_cmd_pub, + $p.UART9, + $p.PG0, + $p.PG1, + $p.DMA2_CH4, + $p.DMA2_CH5, + ); }; } @@ -27,18 +43,32 @@ pub const POWER_RX_BUF_DEPTH: usize = 4; static_idle_buffered_uart!(POWER, POWER_MAX_RX_PACKET_SIZE, POWER_RX_BUF_DEPTH, POWER_MAX_TX_PACKET_SIZE, POWER_TX_BUF_DEPTH, DEBUG_POWER_UART_QUEUES, #[link_section = ".axisram.buffers"]); - pub struct PowerTask< - const POWER_MAX_TX_PACKET_SIZE: usize, - const POWER_MAX_RX_PACKET_SIZE: usize, - const POWER_TX_BUF_DEPTH: usize, - const POWER_RX_BUF_DEPTH: usize> { + const POWER_MAX_TX_PACKET_SIZE: usize, + const POWER_MAX_RX_PACKET_SIZE: usize, + const POWER_TX_BUF_DEPTH: usize, + const POWER_RX_BUF_DEPTH: usize, +> { shared_robot_state: &'static SharedRobotState, power_telemetry_publisher: PowerTelemetryPublisher, led_cmd_publisher: LedCommandPublisher, - _power_uart: &'static IdleBufferedUart, - power_rx_uart_queue: &'static UartReadQueue, - power_tx_uart_queue: &'static UartWriteQueue, + _power_uart: &'static IdleBufferedUart< + POWER_MAX_RX_PACKET_SIZE, + POWER_RX_BUF_DEPTH, + POWER_MAX_TX_PACKET_SIZE, + POWER_TX_BUF_DEPTH, + DEBUG_POWER_UART_QUEUES, + >, + power_rx_uart_queue: &'static UartReadQueue< + POWER_MAX_RX_PACKET_SIZE, + POWER_RX_BUF_DEPTH, + DEBUG_POWER_UART_QUEUES, + >, + power_tx_uart_queue: &'static UartWriteQueue< + POWER_MAX_TX_PACKET_SIZE, + POWER_TX_BUF_DEPTH, + DEBUG_POWER_UART_QUEUES, + >, last_power_status_time: Option, last_power_status: PowerTelemetry, } @@ -47,9 +77,15 @@ impl< const POWER_MAX_TX_PACKET_SIZE: usize, const POWER_MAX_RX_PACKET_SIZE: usize, const POWER_TX_BUF_DEPTH: usize, - const POWER_RX_BUF_DEPTH: usize> - PowerTask { - + const POWER_RX_BUF_DEPTH: usize, + > + PowerTask< + POWER_MAX_TX_PACKET_SIZE, + POWER_MAX_RX_PACKET_SIZE, + POWER_TX_BUF_DEPTH, + POWER_RX_BUF_DEPTH, + > +{ async fn power_task_entry(&mut self) { defmt::info!("power task startup"); @@ -60,19 +96,24 @@ impl< // let mut next_connection_state = self.connection_state; loop { self.process_packets(); - if self.last_power_status_time.is_some() && self.last_power_status_time.unwrap().elapsed() < Duration::from_millis(1000) { + if self.last_power_status_time.is_some() + && self.last_power_status_time.unwrap().elapsed() < Duration::from_millis(1000) + { self.shared_robot_state.set_power_inop(false); } else { self.shared_robot_state.set_power_inop(true); } - if (self.last_power_status_time.is_some() && self.last_power_status.shutdown_requested() == 1) || self.shared_robot_state.shutdown_requested() { + if (self.last_power_status_time.is_some() + && self.last_power_status.shutdown_requested() == 1) + || self.shared_robot_state.shutdown_requested() + { self.try_shutdown().await; } let mut cmd: PowerCommand = Default::default(); cmd.set_request_shutdown(self.shared_robot_state.shutdown_requested() as u32); - + // load any items into command self.send_command(cmd).await; @@ -85,9 +126,12 @@ impl< self.shared_robot_state.flag_shutdown_requested(); - self.led_cmd_publisher.publish(ControlBoardLedCommand::General(ControlGeneralLedCommand::ShutdownRequested)).await; + self.led_cmd_publisher + .publish(ControlBoardLedCommand::General( + ControlGeneralLedCommand::ShutdownRequested, + )) + .await; - // wait for tasks to flag shutdown complete, power board will // hard temrinate after hard after 30s shutdown time loop { @@ -98,13 +142,15 @@ impl< self.send_command(cmd).await; defmt::info!("waiting for kicker board to complete discharge"); - if self.shared_robot_state.kicker_shutdown_complete() || self.shared_robot_state.get_kicker_inop() { + if self.shared_robot_state.kicker_shutdown_complete() + || self.shared_robot_state.get_kicker_inop() + { break; } Timer::after_millis(100).await; } - + Timer::after_millis(100).await; loop { @@ -126,7 +172,12 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("Power - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); + defmt::warn!( + "Power - Got invalid packet of len {:?} (expected {:?}) data: {:?}", + buf.len(), + core::mem::size_of::(), + buf + ); continue; } @@ -141,7 +192,8 @@ impl< } self.last_power_status = status_packet; - self.power_telemetry_publisher.publish_immediate(self.last_power_status); + self.power_telemetry_publisher + .publish_immediate(self.last_power_status); self.last_power_status_time = Some(Instant::now()); } } @@ -169,28 +221,43 @@ pub fn power_uart_config() -> usart::Config { } #[embassy_executor::task] -async fn power_task_entry(mut power_task: PowerTask) { +async fn power_task_entry( + mut power_task: PowerTask< + POWER_MAX_TX_PACKET_SIZE, + POWER_MAX_RX_PACKET_SIZE, + POWER_TX_BUF_DEPTH, + POWER_RX_BUF_DEPTH, + >, +) { loop { power_task.power_task_entry().await; defmt::error!("power task returned"); } } -pub fn start_power_task(power_task_spawner: Spawner, - uart_queue_spawner: SendSpawner, - robot_state: &'static SharedRobotState, - power_telemetry_publisher: PowerTelemetryPublisher, - led_command_publisher: LedCommandPublisher, - power_uart: PowerUart, - power_uart_rx_pin: PowerUartRxPin, - power_uart_tx_pin: PowerUartTxPin, - power_uart_rx_dma: PowerRxDma, - power_uart_tx_dma: PowerTxDma, - ) { - - +pub fn start_power_task( + power_task_spawner: Spawner, + uart_queue_spawner: SendSpawner, + robot_state: &'static SharedRobotState, + power_telemetry_publisher: PowerTelemetryPublisher, + led_command_publisher: LedCommandPublisher, + power_uart: PowerUart, + power_uart_rx_pin: PowerUartRxPin, + power_uart_tx_pin: PowerUartTxPin, + power_uart_rx_dma: PowerRxDma, + power_uart_tx_dma: PowerTxDma, +) { let uart_config = power_uart_config(); - let power_uart = Uart::new(power_uart, power_uart_rx_pin, power_uart_tx_pin, SystemIrqs, power_uart_tx_dma, power_uart_rx_dma, uart_config).unwrap(); + let power_uart = Uart::new( + power_uart, + power_uart_rx_pin, + power_uart_tx_pin, + SystemIrqs, + power_uart_tx_dma, + power_uart_rx_dma, + uart_config, + ) + .unwrap(); defmt::trace!("Power UART initialized"); @@ -211,6 +278,8 @@ pub fn start_power_task(power_task_spawner: Spawner, last_power_status: unsafe { MaybeUninit::zeroed().assume_init() }, }; - power_task_spawner.spawn(power_task_entry(power_task)).unwrap(); + power_task_spawner + .spawn(power_task_entry(power_task)) + .unwrap(); defmt::info!("power task online"); -} \ No newline at end of file +} diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 00ebd98c..8ed2573f 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,16 +1,26 @@ - use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket}; -use ateam_lib_stm32::{idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ gpio::{Input, Pin, Pull}, - usart::{self, DataBits, Parity, StopBits, Uart} + usart::{self, DataBits, Parity, StopBits, Uart}, }; use embassy_time::{Duration, Instant, Ticker, Timer}; -use crate::{create_error_telemetry_from_string, drivers::radio_robot::{RobotRadio, TeamColor}, is_command_packet_safe, pins::*, robot_state::SharedRobotState, tasks::dotstar_task::{ControlBoardLedCommand, RadioStatusLedCommand}, SystemIrqs, DEBUG_RADIO_UART_QUEUES}; +use crate::{ + create_error_telemetry_from_string, + drivers::radio_robot::{RobotRadio, TeamColor}, + is_command_packet_safe, + pins::*, + robot_state::SharedRobotState, + tasks::dotstar_task::{ControlBoardLedCommand, RadioStatusLedCommand}, + SystemIrqs, DEBUG_RADIO_UART_QUEUES, +}; #[macro_export] macro_rules! create_radio_task { @@ -18,13 +28,25 @@ macro_rules! create_radio_task { $radio_command_publisher:ident, $radio_telemetry_subscriber:ident, $led_command_pub:ident, $wifi_credentials:ident, $p:ident) => { ateam_control_board::tasks::radio_task::start_radio_task( - $main_spawner, $rx_uart_queue_spawner, $tx_uart_queue_spawner, + $main_spawner, + $rx_uart_queue_spawner, + $tx_uart_queue_spawner, $robot_state, - $radio_command_publisher, $radio_telemetry_subscriber, $led_command_pub, + $radio_command_publisher, + $radio_telemetry_subscriber, + $led_command_pub, &$wifi_credentials, - $p.USART2, $p.PD6, $p.PD5, $p.PD3, $p.PD4, - $p.DMA2_CH1, $p.DMA2_CH0, - $p.PD7, $p.PA15).await; + $p.USART2, + $p.PD6, + $p.PD5, + $p.PD3, + $p.PD4, + $p.DMA2_CH1, + $p.DMA2_CH0, + $p.PD7, + $p.PA15, + ) + .await; }; } @@ -37,27 +59,34 @@ pub const RADIO_RX_BUF_DEPTH: usize = 4; static_idle_buffered_uart!(RADIO, RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, DEBUG_RADIO_UART_QUEUES, #[link_section = ".axisram.buffers"]); - #[derive(Clone, Copy, PartialEq, PartialOrd, Debug)] enum RadioConnectionState { Unconnected, ConnectedPhys, ConnectedUart, ConnectedNetwork, - Connected + Connected, } pub struct RadioTask< - const RADIO_MAX_TX_PACKET_SIZE: usize, - const RADIO_MAX_RX_PACKET_SIZE: usize, - const RADIO_TX_BUF_DEPTH: usize, - const RADIO_RX_BUF_DEPTH: usize, - const DEBUG_UART_QUEUES: bool> { + const RADIO_MAX_TX_PACKET_SIZE: usize, + const RADIO_MAX_RX_PACKET_SIZE: usize, + const RADIO_TX_BUF_DEPTH: usize, + const RADIO_RX_BUF_DEPTH: usize, + const DEBUG_UART_QUEUES: bool, +> { shared_robot_state: &'static SharedRobotState, command_publisher: CommandsPublisher, telemetry_subscriber: TelemetrySubcriber, led_command_pub: LedCommandPublisher, - radio: RobotRadio<'static, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH, DEBUG_UART_QUEUES>, + radio: RobotRadio< + 'static, + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, radio_ndet_input: Input<'static>, connection_state: RadioConnectionState, @@ -73,21 +102,38 @@ impl< const RADIO_MAX_RX_PACKET_SIZE: usize, const RADIO_TX_BUF_DEPTH: usize, const RADIO_RX_BUF_DEPTH: usize, - const DEBUG_UART_QUEUES: bool> - RadioTask { + const DEBUG_UART_QUEUES: bool, + > + RadioTask< + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, + DEBUG_UART_QUEUES, + > +{ // pub type TaskRobotRadio = RobotRadio<'static, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH>; const RETRY_DELAY_MS: u64 = 1000; const RESPONSE_FROM_PC_TIMEOUT_MS: u64 = 1000; const UART_CONNECT_TIMEOUT_MS: u64 = 5000; - pub fn new(robot_state: &'static SharedRobotState, - command_publisher: CommandsPublisher, - telemetry_subscriber: TelemetrySubcriber, - led_command_pub: LedCommandPublisher, - radio: RobotRadio<'static, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH, DEBUG_UART_QUEUES>, - radio_ndet_input: Input<'static>, - wifi_credentials: &'static [WifiCredential]) -> Self { + pub fn new( + robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + led_command_pub: LedCommandPublisher, + radio: RobotRadio< + 'static, + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, + radio_ndet_input: Input<'static>, + wifi_credentials: &'static [WifiCredential], + ) -> Self { RadioTask { shared_robot_state: robot_state, command_publisher: command_publisher, @@ -103,22 +149,51 @@ impl< } } - pub fn new_from_pins(robot_state: &'static SharedRobotState, - command_publisher: CommandsPublisher, - telemetry_subscriber: TelemetrySubcriber, - led_command_pub: LedCommandPublisher, - radio_uart: &'static IdleBufferedUart, - radio_rx_uart_queue: &'static UartReadQueue, - radio_tx_uart_queue: &'static UartWriteQueue, - radio_reset_pin: impl Pin, - radio_ndet_pin: impl Pin, - wifi_credentials: &'static [WifiCredential]) -> Self { - - let radio = RobotRadio::new(radio_uart, radio_rx_uart_queue, radio_tx_uart_queue, radio_reset_pin, robot_state.hw_wifi_driver_use_flow_control()); + pub fn new_from_pins( + robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + led_command_pub: LedCommandPublisher, + radio_uart: &'static IdleBufferedUart< + RADIO_MAX_RX_PACKET_SIZE, + RADIO_RX_BUF_DEPTH, + RADIO_MAX_TX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, + radio_rx_uart_queue: &'static UartReadQueue< + RADIO_MAX_RX_PACKET_SIZE, + RADIO_RX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, + radio_tx_uart_queue: &'static UartWriteQueue< + RADIO_MAX_TX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, + radio_reset_pin: impl Pin, + radio_ndet_pin: impl Pin, + wifi_credentials: &'static [WifiCredential], + ) -> Self { + let radio = RobotRadio::new( + radio_uart, + radio_rx_uart_queue, + radio_tx_uart_queue, + radio_reset_pin, + robot_state.hw_wifi_driver_use_flow_control(), + ); let radio_ndet = Input::new(radio_ndet_pin, Pull::None); - Self::new(robot_state, command_publisher, telemetry_subscriber, led_command_pub, radio, radio_ndet, wifi_credentials) + Self::new( + robot_state, + command_publisher, + telemetry_subscriber, + led_command_pub, + radio, + radio_ndet, + wifi_credentials, + ) } async fn radio_task_entry(&mut self) { @@ -138,7 +213,7 @@ impl< loop { tx_ctr += 1; tx_ctr &= 0x03; - + let loop_start_time = Instant::now(); let loop_invocation_dead_time = loop_start_time - last_loop_term_time; if loop_start_time - last_loop_term_time > Duration::from_millis(11) { @@ -153,17 +228,21 @@ impl< let cur_robot_state = self.shared_robot_state.get_state(); if cur_robot_state != last_robot_state { // we are connected to software and robot id or team was changed on hardware - if self.connection_state == RadioConnectionState::Connected && - (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id || - cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) { + if self.connection_state == RadioConnectionState::Connected + && (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id + || cur_robot_state.hw_robot_team_is_blue + != last_robot_state.hw_robot_team_is_blue) + { // Enter connected network state (disconnecting) self.connection_state = RadioConnectionState::ConnectedNetwork; defmt::info!("shell state change triggering software reconnect"); } // We are at least connected on UART and the wifi network id changed - if self.connection_state >= RadioConnectionState::ConnectedUart && - cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { + if self.connection_state >= RadioConnectionState::ConnectedUart + && cur_robot_state.hw_wifi_network_index + != last_robot_state.hw_wifi_network_index + { defmt::info!("dip state change triggering wifi network change"); if self.radio.disconnect_network().await.is_err() { @@ -183,7 +262,9 @@ impl< defmt::error!("radio appears unplugged."); radio_inop_flag_local = true; self.connection_state = RadioConnectionState::Unconnected; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::Off)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::Off)) + .await; } // execute on the connection state @@ -196,22 +277,34 @@ impl< } else { // Pin is detected, so connected physically. self.connection_state = RadioConnectionState::ConnectedPhys; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedPhys)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedPhys, + )) + .await; } - }, + } RadioConnectionState::ConnectedPhys => { if self.connect_uart().await.is_err() { radio_inop_flag_local = true; - // If the pin is unconnected, will be overridden out of the state. + // If the pin is unconnected, will be overridden out of the state. // So just check UART again. self.connection_state = RadioConnectionState::ConnectedPhys; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedPhys)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedPhys, + )) + .await; } else { // UART is not in error, so good to go. self.connection_state = RadioConnectionState::ConnectedUart; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedUart)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedUart, + )) + .await; } - }, + } RadioConnectionState::ConnectedUart => { let wifi_network_ind = cur_robot_state.hw_wifi_network_index as usize; let wifi_credential = if wifi_network_ind >= self.wifi_credentials.len() { @@ -220,64 +313,106 @@ impl< self.wifi_credentials[wifi_network_ind] }; - defmt::debug!("connecting to network ({}): ssid: {}, password: {}", wifi_network_ind, wifi_credential.get_ssid(), wifi_credential.get_password()); - if self.connect_network(wifi_credential, cur_robot_state.hw_robot_id).await.is_err() { + defmt::debug!( + "connecting to network ({}): ssid: {}, password: {}", + wifi_network_ind, + wifi_credential.get_ssid(), + wifi_credential.get_password() + ); + if self + .connect_network(wifi_credential, cur_robot_state.hw_robot_id) + .await + .is_err() + { // If network connection failed, go back up to verify UART. radio_network_fail_local = true; self.connection_state = RadioConnectionState::ConnectedPhys; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedPhys)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedPhys, + )) + .await; } else { self.connection_state = RadioConnectionState::ConnectedNetwork; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedNetwork)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedNetwork, + )) + .await; } - }, + } RadioConnectionState::ConnectedNetwork => { - if let Ok(connected) = self.connect_software(cur_robot_state.hw_robot_id, cur_robot_state.hw_robot_team_is_blue).await { + if let Ok(connected) = self + .connect_software( + cur_robot_state.hw_robot_id, + cur_robot_state.hw_robot_team_is_blue, + ) + .await + { if connected { // Refresh last software packet on first connect. self.last_software_packet = Instant::now(); self.connection_state = RadioConnectionState::Connected; radio_loop_rate_ticker.reset(); - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedSoftware)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedSoftware, + )) + .await; } else { // Software didn't respond to our hello, it may not be started yet. radio_network_fail_local = true; self.connection_state = RadioConnectionState::ConnectedNetwork; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedNetwork)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedNetwork, + )) + .await; } } else { radio_network_fail_local = true; // If network connection failed, go back up to verify UART. self.connection_state = RadioConnectionState::ConnectedPhys; - self.led_command_pub.publish(ControlBoardLedCommand::Radio(RadioStatusLedCommand::ConnectedPhys)).await; + self.led_command_pub + .publish(ControlBoardLedCommand::Radio( + RadioStatusLedCommand::ConnectedPhys, + )) + .await; } - }, + } RadioConnectionState::Connected => { let _ = self.process_packets(tx_ctr).await; // if we're stably connected, process packets at 100Hz - // If timeout have elapsed since we last got a packet, + // If timeout have elapsed since we last got a packet, // 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() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { - defmt::warn!("software timeout - rebooting..."); - Timer::after_millis(100).await; + if !cur_robot_state.shutdown_requested + && Instant::checked_duration_since(&cur_time, self.last_software_packet) + .unwrap() + .as_millis() + > Self::RESPONSE_FROM_PC_TIMEOUT_MS + { + defmt::warn!("software timeout - rebooting..."); + Timer::after_millis(100).await; cortex_m::peripheral::SCB::sys_reset(); } - }, + } } - // set global radio connected flag - self.shared_robot_state.set_radio_network_ok(self.connection_state >= RadioConnectionState::ConnectedNetwork); - self.shared_robot_state.set_radio_bridge_ok(self.connection_state == RadioConnectionState::Connected); - self.shared_robot_state.set_radio_inop(radio_inop_flag_local); + // set global radio connected flag + self.shared_robot_state.set_radio_network_ok( + self.connection_state >= RadioConnectionState::ConnectedNetwork, + ); + self.shared_robot_state + .set_radio_bridge_ok(self.connection_state == RadioConnectionState::Connected); + self.shared_robot_state + .set_radio_inop(radio_inop_flag_local); if radio_inop_flag_local { // If hardware problems is present, adds a delay. Timer::after_millis(Self::RETRY_DELAY_MS).await; - } - else if radio_network_fail_local { + } else if radio_network_fail_local { // If network problems is present, adds a delay. Timer::after_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS).await; } @@ -294,12 +429,16 @@ impl< radio_loop_rate_ticker.next().await; } - } async fn connect_uart(&mut self) -> Result<(), ()> { defmt::info!("connecting radio uart"); - match select(self.radio.connect_uart(), Timer::after_millis(Self::UART_CONNECT_TIMEOUT_MS)).await { + match select( + self.radio.connect_uart(), + Timer::after_millis(Self::UART_CONNECT_TIMEOUT_MS), + ) + .await + { Either::First(res) => { if res.is_err() { defmt::error!("failed to establish radio UART connection."); @@ -316,13 +455,22 @@ impl< } } - async fn connect_network(&mut self, wifi_network: WifiCredential, robot_id: u8) -> Result<(), ()> { - if self.radio.connect_to_network(wifi_network, robot_id).await.is_err() { + async fn connect_network( + &mut self, + wifi_network: WifiCredential, + robot_id: u8, + ) -> Result<(), ()> { + if self + .radio + .connect_to_network(wifi_network, robot_id) + .await + .is_err() + { defmt::error!("failed to connect to wifi network."); return Err(()); } defmt::info!("radio connected"); - + let res = self.radio.open_multicast().await; if res.is_err() { defmt::error!("failed to establish multicast socket to network."); @@ -346,24 +494,36 @@ impl< defmt::error!("send hello failed."); return Err(()); } - - let hello = self.radio.wait_hello(Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS)).await; + + let hello = self + .radio + .wait_hello(Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS)) + .await; match hello { Ok(hello) => { defmt::info!( "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port + hello.ipv4[0], + hello.ipv4[1], + hello.ipv4[2], + hello.ipv4[3], + hello.port ); let start_time = Instant::now(); self.radio.close_peer().await.unwrap(); - - self.radio.open_unicast(hello.ipv4, hello.port).await.expect("failed to open unicast port"); + + self.radio + .open_unicast(hello.ipv4, hello.port) + .await + .expect("failed to open unicast port"); defmt::info!("unicast open"); let end_time = Instant::now(); - defmt::info!("multicast peer closed (took {} ms)", (end_time - start_time).as_millis()); - + defmt::info!( + "multicast peer closed (took {} ms)", + (end_time - start_time).as_millis() + ); return Ok(true); } @@ -384,8 +544,15 @@ impl< self.command_publisher.publish_immediate(c2_pkt); } else { defmt::error!("RadioTask - received unsafe packet"); - let error_telemetry = create_error_telemetry_from_string("Received unsafe command packet - check for NaNs"); - if self.radio.send_error_telemetry(error_telemetry).await.is_err() { + let error_telemetry = create_error_telemetry_from_string( + "Received unsafe command packet - check for NaNs", + ); + if self + .radio + .send_error_telemetry(error_telemetry) + .await + .is_err() + { defmt::warn!("RadioTask - failed to send error telemetry packet"); } } @@ -403,20 +570,29 @@ impl< match telemetry { TelemetryPacket::Basic(basic) => { self.last_basic_telemetry = basic; - }, + } TelemetryPacket::Extended(control) => { if self.shared_robot_state.get_radio_send_extended_telem() { // defmt::info!("RadioTask - sending extended telemetry"); - if self.radio.send_control_debug_telemetry(control).await.is_err() { - defmt::warn!("RadioTask - failed to send control debug telemetry packet"); + if self + .radio + .send_control_debug_telemetry(control) + .await + .is_err() + { + defmt::warn!( + "RadioTask - failed to send control debug telemetry packet" + ); } } - }, + } TelemetryPacket::ParameterCommandResponse(pc_resp) => { if self.radio.send_parameter_response(pc_resp).await.is_err() { - defmt::warn!("RadioTask - failed to send control parameter response packet"); + defmt::warn!( + "RadioTask - failed to send control parameter response packet" + ); } - }, + } TelemetryPacket::ErrorTelemetry(error_packet) => { if self.radio.send_error_telemetry(error_packet).await.is_err() { defmt::warn!("RadioTask - failed to send error packet"); @@ -440,7 +616,7 @@ impl< } } - return Ok(()) + return Ok(()); } } @@ -455,31 +631,40 @@ pub fn startup_uart_config() -> usart::Config { } #[embassy_executor::task] -async fn radio_task_entry(mut radio_task: RadioTask) { +async fn radio_task_entry( + mut radio_task: RadioTask< + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, + DEBUG_RADIO_UART_QUEUES, + >, +) { loop { radio_task.radio_task_entry().await; defmt::error!("radio task returned"); } } -pub async fn start_radio_task(radio_task_spawner: Spawner, - rx_queue_spawner: SendSpawner, - tx_queue_spawner: SendSpawner, - robot_state: &'static SharedRobotState, - command_publisher: CommandsPublisher, - telemetry_subscriber: TelemetrySubcriber, - led_command_pub: LedCommandPublisher, - wifi_credentials: &'static [WifiCredential], - radio_uart: RadioUART, - radio_uart_rx_pin: RadioUartRxPin, - radio_uart_tx_pin: RadioUartTxPin, - _radio_uart_cts_pin: RadioUartCtsPin, - _radio_uart_rts_pin: RadioUartRtsPin, - radio_uart_rx_dma: RadioRxDMA, - radio_uart_tx_dma: RadioTxDMA, - radio_reset_pin: RadioResetPin, - radio_ndet_pin: RadioNDetectPin) { - +pub async fn start_radio_task( + radio_task_spawner: Spawner, + rx_queue_spawner: SendSpawner, + tx_queue_spawner: SendSpawner, + robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + led_command_pub: LedCommandPublisher, + wifi_credentials: &'static [WifiCredential], + radio_uart: RadioUART, + radio_uart_rx_pin: RadioUartRxPin, + radio_uart_tx_pin: RadioUartTxPin, + _radio_uart_cts_pin: RadioUartCtsPin, + _radio_uart_rts_pin: RadioUartRtsPin, + radio_uart_rx_dma: RadioRxDMA, + radio_uart_tx_dma: RadioTxDMA, + radio_reset_pin: RadioResetPin, + radio_ndet_pin: RadioNDetectPin, +) { let uart_conifg = startup_uart_config(); // wait for IO task to read dip switches @@ -492,13 +677,32 @@ pub async fn start_radio_task(radio_task_spawner: Spawner, } } - let radio_uart = if robot_state.hw_wifi_driver_use_flow_control() { defmt::info!("radio will use flow control"); - Uart::new_with_rtscts(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, _radio_uart_rts_pin, _radio_uart_cts_pin, radio_uart_tx_dma, radio_uart_rx_dma, uart_conifg).unwrap() + Uart::new_with_rtscts( + radio_uart, + radio_uart_rx_pin, + radio_uart_tx_pin, + SystemIrqs, + _radio_uart_rts_pin, + _radio_uart_cts_pin, + radio_uart_tx_dma, + radio_uart_rx_dma, + uart_conifg, + ) + .unwrap() } else { defmt::info!("radio will NOT use flow control"); - Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, uart_conifg).unwrap() + Uart::new( + radio_uart, + radio_uart_rx_pin, + radio_uart_tx_pin, + SystemIrqs, + radio_uart_tx_dma, + radio_uart_rx_dma, + uart_conifg, + ) + .unwrap() }; let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); @@ -507,15 +711,30 @@ pub async fn start_radio_task(radio_task_spawner: Spawner, defmt::info!("uart queue init"); - - rx_queue_spawner.spawn(idle_buffered_uart_read_task!(RADIO, radio_uart_rx)).unwrap(); + rx_queue_spawner + .spawn(idle_buffered_uart_read_task!(RADIO, radio_uart_rx)) + .unwrap(); defmt::info!("radio uart read task online"); - tx_queue_spawner.spawn(idle_buffered_uart_write_task!(RADIO, radio_uart_tx)).unwrap(); + tx_queue_spawner + .spawn(idle_buffered_uart_write_task!(RADIO, radio_uart_tx)) + .unwrap(); defmt::info!("radio uart write task online"); - - let radio_task = RadioTask::new_from_pins(robot_state, command_publisher, telemetry_subscriber, led_command_pub, &RADIO_IDLE_BUFFERED_UART, RADIO_IDLE_BUFFERED_UART.get_uart_read_queue(), RADIO_IDLE_BUFFERED_UART.get_uart_write_queue(), radio_reset_pin, radio_ndet_pin, wifi_credentials); - - radio_task_spawner.spawn(radio_task_entry(radio_task)).unwrap(); + let radio_task = RadioTask::new_from_pins( + robot_state, + command_publisher, + telemetry_subscriber, + led_command_pub, + &RADIO_IDLE_BUFFERED_UART, + RADIO_IDLE_BUFFERED_UART.get_uart_read_queue(), + RADIO_IDLE_BUFFERED_UART.get_uart_write_queue(), + radio_reset_pin, + radio_ndet_pin, + wifi_credentials, + ); + + radio_task_spawner + .spawn(radio_task_entry(radio_task)) + .unwrap(); defmt::info!("radio task online"); -} \ No newline at end of file +} diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 8b8cb7aa..ce6b3e39 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -4,37 +4,62 @@ use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_time::{Duration, Ticker}; +use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; -use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; -use embassy_stm32::adc::{Adc, SampleTime, Resolution}; +use embassy_stm32::adc::{Adc, Resolution, SampleTime}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; -use crate::{adc_v_to_battery_voltage, pins::*, BATTERY_MIN_CRIT_VOLTAGE, BATTERY_MIN_SAFE_VOLTAGE, BATTERY_MAX_VOLTAGE}; +use crate::{ + adc_v_to_battery_voltage, pins::*, BATTERY_MAX_VOLTAGE, BATTERY_MIN_CRIT_VOLTAGE, + BATTERY_MIN_SAFE_VOLTAGE, +}; fn get_vref_int_cal() -> u16 { unsafe { *(0x1FF1_E860 as *const u16) } } -const IO_TASK_LOOP_RATE_DURATION: Duration = Duration::from_millis(50); // 20 Hz +const IO_TASK_LOOP_RATE_DURATION: Duration = Duration::from_millis(50); // 20 Hz const BATTERY_FILTER_WINDOW: Duration = Duration::from_secs(10); #[macro_export] macro_rules! create_io_task { ($spawner:ident, $robot_state:ident, $p:ident) => { - ateam_control_board::tasks::user_io_task::start_io_task(&$spawner, + ateam_control_board::tasks::user_io_task::start_io_task( + &$spawner, $robot_state, - $p.ADC1, $p.PA0, + $p.ADC1, + $p.PA0, $p.ADC3, - $p.PE9, $p.PE8, $p.PE7, $p.PF15, $p.PF14, $p.PF13, $p.PF12, $p.PF11, + $p.PE9, + $p.PE8, + $p.PE7, + $p.PF15, + $p.PF14, + $p.PF13, + $p.PF12, + $p.PF11, $p.PC13, - $p.PB10, $p.PB11, - $p.PD0, $p.PD1, $p.PG10, $p.PG11, - $p.PG6, $p.PG5, $p.PG4, $p.PD13, + $p.PB10, + $p.PB11, + $p.PD0, + $p.PD1, + $p.PG10, + $p.PG11, + $p.PG6, + $p.PG5, + $p.PG4, + $p.PD13, $p.PD10, - $p.PC3, $p.PC1, $p.PC0, $p.PC2, $p.PF10).await; + $p.PC3, + $p.PC1, + $p.PC0, + $p.PC2, + $p.PF10, + ) + .await; }; } @@ -65,15 +90,16 @@ async fn user_io_task_entry( /////////////////////// // Battery ADC Setup // /////////////////////// - + // Get the Vref_int calibration values. let vref_int_cal = get_vref_int_cal() as f32; let mut _vref_int_ch = vref_int_adc.enable_vrefint(); // Create the running buffers for averaging - const BATTERY_VOLTAGE_FILTER_ALPHA: f32 = IO_TASK_LOOP_RATE_DURATION.as_millis() as f32 / BATTERY_FILTER_WINDOW.as_millis() as f32; + const BATTERY_VOLTAGE_FILTER_ALPHA: f32 = + IO_TASK_LOOP_RATE_DURATION.as_millis() as f32 / BATTERY_FILTER_WINDOW.as_millis() as f32; let mut battery_voltage_filter = IirFilter::new(BATTERY_VOLTAGE_FILTER_ALPHA); - + loop { let cur_robot_state = robot_state.get_state(); @@ -83,9 +109,9 @@ async fn user_io_task_entry( let send_extended_telem = dip_switch.read_pin(0); let hw_debug_mode = debug_mode_dip_switch.read_pin(0); - + let robot_team_isblue = robot_color_dip_switch.read_pin(1); - + let robot_id = robot_id_rotary.read_value(); if hw_debug_mode != cur_robot_state.hw_debug_mode { @@ -97,55 +123,85 @@ async fn user_io_task_entry( if robot_radio_driver_use_flow_control != cur_robot_state.hw_wifi_driver_use_flow_control { robot_state.set_hw_wifi_driver_use_flow_control(robot_radio_driver_use_flow_control); - defmt::info!("updated radio driver use flow control {} -> {}", cur_robot_state.hw_wifi_driver_use_flow_control, robot_radio_driver_use_flow_control); + defmt::info!( + "updated radio driver use flow control {} -> {}", + cur_robot_state.hw_wifi_driver_use_flow_control, + robot_radio_driver_use_flow_control + ); } // publish updates to robot_state if robot_id != cur_robot_state.hw_robot_id { robot_state.set_hw_robot_id(robot_id); - defmt::info!("updated robot id {} -> {}", cur_robot_state.hw_robot_id, robot_id); + defmt::info!( + "updated robot id {} -> {}", + cur_robot_state.hw_robot_id, + robot_id + ); } if robot_team_isblue != cur_robot_state.hw_robot_team_is_blue { robot_state.set_hw_robot_team_is_blue(robot_team_isblue); - defmt::info!("updated robot team is blue {} -> {}", cur_robot_state.hw_robot_team_is_blue, robot_team_isblue); + defmt::info!( + "updated robot team is blue {} -> {}", + cur_robot_state.hw_robot_team_is_blue, + robot_team_isblue + ); } if robot_network_index != cur_robot_state.hw_wifi_network_index as u8 { robot_state.set_hw_network_index(robot_network_index); - defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); + defmt::info!( + "updated robot network index {} -> {}", + cur_robot_state.hw_wifi_network_index, + robot_network_index + ); } if send_extended_telem != cur_robot_state.radio_send_extended_telem { robot_state.set_radio_send_extended_telem(send_extended_telem); - defmt::info!("updated robot send extended telem {} -> {}", cur_robot_state.radio_send_extended_telem, send_extended_telem); + defmt::info!( + "updated robot send extended telem {} -> {}", + cur_robot_state.radio_send_extended_telem, + send_extended_telem + ); } - + /////////////////////// // Battery reading // /////////////////////// - // FIXME: Vref_int is not returning valid value. Embassy issue. + // FIXME: Vref_int is not returning valid value. Embassy issue. // let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); let vref_int_read_mv = 1216.0; - let batt_res_div_v = battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal) * 0.9090; + let batt_res_div_v = + battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal) * 0.9090; let cur_battery_voltage = adc_v_to_battery_voltage(batt_res_div_v); - + // Add new battery read to cyclical buffer. - battery_voltage_filter.add_sample(cur_battery_voltage); - let battery_voltage_ave = battery_voltage_filter.filtered_value().expect("iir never return None here"); + battery_voltage_filter.add_sample(cur_battery_voltage); + let battery_voltage_ave = battery_voltage_filter + .filtered_value() + .expect("iir never return None here"); // TODO do something with local voltage? // battery_volt_publisher.publish_immediate(battery_voltage_ave); robot_state.set_battery_low(battery_voltage_ave < BATTERY_MIN_SAFE_VOLTAGE); - robot_state.set_battery_crit(battery_voltage_ave < BATTERY_MIN_CRIT_VOLTAGE || battery_voltage_ave > BATTERY_MAX_VOLTAGE); + robot_state.set_battery_crit( + battery_voltage_ave < BATTERY_MIN_CRIT_VOLTAGE + || battery_voltage_ave > BATTERY_MAX_VOLTAGE, + ); // update indicators robot_id_indicator.set(robot_id, robot_team_isblue); robot_id_src_disagree.set_low(); if !robot_state.hw_init_state_valid() { - defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); + defmt::info!( + "loaded robot state: robot id: {}, team: {}", + robot_id, + robot_team_isblue + ); robot_state.set_hw_init_state_valid(true); } @@ -186,33 +242,65 @@ async fn user_io_task_entry( } } -pub async fn start_io_task(spawner: &Spawner, +pub async fn start_io_task( + spawner: &Spawner, robot_state: &'static SharedRobotState, - battery_adc_peri: BatteryAdc, battery_adc_pin: BatteryAdcPin, + battery_adc_peri: BatteryAdc, + battery_adc_pin: BatteryAdcPin, vref_int_adc_peri: VrefIntAdc, - usr_dip7_pin: UsrDip7Pin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, - usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, usr_dip0_pin: UsrDip0Pin, + usr_dip7_pin: UsrDip7Pin, + usr_dip6_pin: UsrDip6Pin, + usr_dip5_pin: UsrDip5Pin, + usr_dip4_pin: UsrDip4Pin, + usr_dip3_pin: UsrDip3Pin, + usr_dip2_pin: UsrDip2Pin, + usr_dip1_pin: UsrDip1Pin, + usr_dip0_pin: UsrDip0Pin, debug_mode_pin: UsrDipDebugMode, - robot_id_team_is_blue_pin: UsrDipTeamIsBluePin, robot_id_src_select_pin: UsrDipBotIdSrcSelect, - robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, - robot_id_selector1_pin: RobotIdSelector1Pin, robot_id_selector0_pin: RobotIdSelector0Pin, - usr_led0_pin: UsrLed0Pin, usr_led1_pin: UsrLed1Pin, usr_led2_pin: UsrLed2Pin, usr_led3_pin: UsrLed3Pin, + robot_id_team_is_blue_pin: UsrDipTeamIsBluePin, + robot_id_src_select_pin: UsrDipBotIdSrcSelect, + robot_id_selector3_pin: RobotIdSelector3Pin, + robot_id_selector2_pin: RobotIdSelector2Pin, + robot_id_selector1_pin: RobotIdSelector1Pin, + robot_id_selector0_pin: RobotIdSelector0Pin, + usr_led0_pin: UsrLed0Pin, + usr_led1_pin: UsrLed1Pin, + usr_led2_pin: UsrLed2Pin, + usr_led3_pin: UsrLed3Pin, robot_id_src_disagree_led_pin: RobotIdSrcDisagree, - robot_id_indicator_fl: RobotIdIndicator0FlPin, robot_id_indicator_bl: RobotIdIndicator1BlPin, - robot_id_indicator_br: RobotIdIndicator2BrPin, robot_id_indicator_fr: RobotIdIndicator3FrPin, + robot_id_indicator_fl: RobotIdIndicator0FlPin, + robot_id_indicator_bl: RobotIdIndicator1BlPin, + robot_id_indicator_br: RobotIdIndicator2BrPin, + robot_id_indicator_fr: RobotIdIndicator3FrPin, robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, - ) { - - let dip_sw_pins: [AnyPin; 8] = [usr_dip7_pin.into(), usr_dip6_pin.into(), usr_dip5_pin.into(), usr_dip4_pin.into(), usr_dip3_pin.into(), usr_dip2_pin.into(), usr_dip1_pin.into(), usr_dip0_pin.into()]; +) { + let dip_sw_pins: [AnyPin; 8] = [ + usr_dip7_pin.into(), + usr_dip6_pin.into(), + usr_dip5_pin.into(), + usr_dip4_pin.into(), + usr_dip3_pin.into(), + usr_dip2_pin.into(), + usr_dip1_pin.into(), + usr_dip0_pin.into(), + ]; let dip_switch = DipSwitch::new_from_pins(dip_sw_pins, Pull::None, None); let debug_mode_pins: [AnyPin; 1] = [debug_mode_pin.into()]; let debug_mode_dip = DipSwitch::new_from_pins(debug_mode_pins, Pull::None, Some([true])); - - let team_color_pins: [AnyPin; 2] = [robot_id_src_select_pin.into(), robot_id_team_is_blue_pin.into()]; + + let team_color_pins: [AnyPin; 2] = [ + robot_id_src_select_pin.into(), + robot_id_team_is_blue_pin.into(), + ]; let team_color_dip = DipSwitch::new_from_pins(team_color_pins, Pull::None, Some([false, true])); - let robot_id_selector_pins: [AnyPin; 4] = [robot_id_selector3_pin.into(), robot_id_selector2_pin.into(), robot_id_selector1_pin.into(), robot_id_selector0_pin.into()]; + let robot_id_selector_pins: [AnyPin; 4] = [ + robot_id_selector3_pin.into(), + robot_id_selector2_pin.into(), + robot_id_selector1_pin.into(), + robot_id_selector0_pin.into(), + ]; let robot_id_rotary = RotaryEncoder::new_from_pins(robot_id_selector_pins, Pull::None, None); let debug_led0 = Output::new(usr_led0_pin, Level::Low, Speed::Low); @@ -220,19 +308,42 @@ pub async fn start_io_task(spawner: &Spawner, let debug_led2 = Output::new(usr_led2_pin, Level::Low, Speed::Low); let debug_led3 = Output::new(usr_led3_pin, Level::Low, Speed::Low); - let robot_id_src_disagree_led = Output::new(robot_id_src_disagree_led_pin, Level::Low, Speed::Low); - let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_bl, robot_id_indicator_br, Some(robot_id_indicator_isblue)); - - let battery_volt_adc = AdcHelper::new(battery_adc_peri, battery_adc_pin, SampleTime::CYCLES32_5, Resolution::BITS12); + let robot_id_src_disagree_led = + Output::new(robot_id_src_disagree_led_pin, Level::Low, Speed::Low); + let robot_id_indicator = ShellIndicator::new( + robot_id_indicator_fr, + robot_id_indicator_fl, + robot_id_indicator_bl, + robot_id_indicator_br, + Some(robot_id_indicator_isblue), + ); + + let battery_volt_adc = AdcHelper::new( + battery_adc_peri, + battery_adc_pin, + SampleTime::CYCLES32_5, + Resolution::BITS12, + ); let mut vref_int_adc = Adc::new(vref_int_adc_peri); // Set the Vref_int ADC settings to the same as the battery. vref_int_adc.set_resolution(Resolution::BITS12); vref_int_adc.set_sample_time(SampleTime::CYCLES32_5); - spawner.spawn(user_io_task_entry( - robot_state, - battery_volt_adc, vref_int_adc, - dip_switch, debug_mode_dip, team_color_dip, robot_id_rotary, - debug_led0, debug_led1, debug_led2, debug_led3, - robot_id_src_disagree_led, robot_id_indicator)).unwrap(); -} \ No newline at end of file + spawner + .spawn(user_io_task_entry( + robot_state, + battery_volt_adc, + vref_int_adc, + dip_switch, + debug_mode_dip, + team_color_dip, + robot_id_rotary, + debug_led0, + debug_led1, + debug_led2, + debug_led3, + robot_id_src_disagree_led, + robot_id_indicator, + )) + .unwrap(); +} diff --git a/control-board/tests/basic-test.rs b/control-board/tests/basic-test.rs index 868aea85..d29f4560 100644 --- a/control-board/tests/basic-test.rs +++ b/control-board/tests/basic-test.rs @@ -1,9 +1,9 @@ #![no_std] #![no_main] +use defmt_rtt as _; use embassy_stm32 as _; use panic_probe as _; -use defmt_rtt as _; #[defmt_test::tests] mod tests { @@ -13,4 +13,4 @@ mod tests { fn it_works() { assert!(true) } -} \ No newline at end of file +} diff --git a/control-board/tests/drivetrain-test.rs b/control-board/tests/drivetrain-test.rs index 091a2f93..f2391e26 100644 --- a/control-board/tests/drivetrain-test.rs +++ b/control-board/tests/drivetrain-test.rs @@ -3,21 +3,21 @@ #![feature(const_mut_refs)] #![feature(async_closure)] +use ateam_control_board::{ + queue::{self, Buffer}, + stm32_interface::Stm32Interface, + uart_queue::{UartReadQueue, UartWriteQueue}, +}; use defmt_rtt as _; use embassy_stm32::{ self, executor::InterruptExecutor, - interrupt::{self, InterruptExt}, gpio::{Level, OutputOpenDrain, Pin, Pull, Speed}, + interrupt::{self, InterruptExt}, peripherals::{DMA1_CH0, DMA1_CH1, DMA1_CH2, DMA1_CH3, DMA1_CH4, DMA1_CH5, DMA1_CH6, DMA1_CH7}, - peripherals::{USART3, UART4, UART5, UART7}, + peripherals::{UART4, UART5, UART7, USART3}, usart::{self, Uart}, }; -use ateam_control_board::{ - stm32_interface::Stm32Interface, - queue::{self, Buffer}, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; use panic_probe as _; use static_cell::StaticCell; diff --git a/control-board/tests/uart-queue.rs b/control-board/tests/uart-queue.rs index 5faeccc9..6e07b390 100644 --- a/control-board/tests/uart-queue.rs +++ b/control-board/tests/uart-queue.rs @@ -3,6 +3,10 @@ #![feature(const_mut_refs)] #![feature(async_closure)] +use ateam_control_board::{ + queue::{self, Buffer}, + uart_queue::{UartReadQueue, UartWriteQueue}, +}; use defmt_rtt as _; use embassy_stm32::{ self, @@ -11,10 +15,6 @@ use embassy_stm32::{ peripherals::{DMA1_CH0, DMA1_CH1, UART7}, usart::{self, Uart}, }; -use ateam_control_board::{ - queue::{self, Buffer}, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; use panic_probe as _; use static_cell::StaticCell; diff --git a/credentials/src/lib.rs b/credentials/src/lib.rs index aef6ab76..255e9354 100644 --- a/credentials/src/lib.rs +++ b/credentials/src/lib.rs @@ -1,9 +1,9 @@ #![no_std] #![allow(non_upper_case_globals)] -pub mod public_credentials; #[cfg(not(feature = "no-private-credentials"))] pub mod private_credentials; +pub mod public_credentials; #[derive(Clone, Copy)] pub struct WifiCredential { @@ -20,5 +20,3 @@ impl WifiCredential { self.password } } - - diff --git a/credentials/src/public_credentials/mod.rs b/credentials/src/public_credentials/mod.rs index 3c687b22..3b0e25e4 100644 --- a/credentials/src/public_credentials/mod.rs +++ b/credentials/src/public_credentials/mod.rs @@ -1 +1 @@ -pub mod wifi; \ No newline at end of file +pub mod wifi; diff --git a/credentials/src/public_credentials/wifi.rs b/credentials/src/public_credentials/wifi.rs index 39178346..ae20705c 100644 --- a/credentials/src/public_credentials/wifi.rs +++ b/credentials/src/public_credentials/wifi.rs @@ -8,6 +8,12 @@ pub enum WifiCredentialIndices { } pub static wifi_credentials: [WifiCredential; WifiCredentialIndices::COUNT as usize] = [ - WifiCredential { ssid: "Dummy Field Network", password: "password1" }, - WifiCredential { ssid: "Dummy Development Network", password: "password2" }, -]; \ No newline at end of file + WifiCredential { + ssid: "Dummy Field Network", + password: "password1", + }, + WifiCredential { + ssid: "Dummy Development Network", + password: "password2", + }, +]; diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 52199be4..9d269294 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -5,36 +5,37 @@ use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use embassy_executor::Spawner; use embassy_executor::Executor; +use embassy_executor::Spawner; use embassy_stm32::{ adc::{Adc, SampleTime}, - gpio::{Input, Level, Output, Pull, Speed}, - spi::{Config, Spi}, time::mhz, + gpio::{Input, Level, Output, Pull, Speed}, + spi::{Config, Spi}, + time::mhz, }; use embassy_time::{Duration, Timer}; use static_cell::StaticCell; -use ateam_kicker_board::{tasks::get_system_config, *}; use ateam_kicker_board::pins::*; +use ateam_kicker_board::{tasks::get_system_config, *}; use panic_probe as _; // use panic_halt as _; #[embassy_executor::task] -async fn blink( - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - status_led_blue1: BlueStatusLedPin, - usr_btn_pin: UserBtnPin, - mut adc: Adc<'static, PowerRailAdc>, - mut rail_200v_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - mut rail_5v0_pin: PowerRail5v0ReadPin, - mut rail_3v3_pin: PowerRail3v3ReadPin) -> ! { - +async fn blink( + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + status_led_blue1: BlueStatusLedPin, + usr_btn_pin: UserBtnPin, + mut adc: Adc<'static, PowerRailAdc>, + mut rail_200v_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRailVswReadPin, + mut rail_5v0_pin: PowerRail5v0ReadPin, + mut rail_3v3_pin: PowerRail3v3ReadPin, +) -> ! { let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::High, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); @@ -81,12 +82,13 @@ async fn blink( // adc_12v_to_rail_voltage(raw_12v), // adc_5v0_to_rail_voltage(raw_5v0), // adc_3v3_to_rail_voltage(raw_3v3)); - defmt::info!("voltages - 200v ({}), Vsw ({}), 5v0 ({}), 3v3 ({})", + defmt::info!( + "voltages - 200v ({}), Vsw ({}), 5v0 ({}), 3v3 ({})", adc_200v_to_rail_voltage(adc_raw_to_v(raw_200v, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(raw_12v, vrefint_sample)), adc_5v0_to_rail_voltage(adc_raw_to_v(raw_5v0, vrefint_sample)), - adc_3v3_to_rail_voltage(adc_raw_to_v(raw_3v3, vrefint_sample))); - + adc_3v3_to_rail_voltage(adc_raw_to_v(raw_3v3, vrefint_sample)) + ); } } @@ -95,8 +97,8 @@ async fn dotstar_lerp_task( dotstar_spi: DotstarSpi, dotstar_mosi_pin: DotstarSpiMosiPin, dotstar_sck_pin: DotstarSpiSckPin, - dotstar_tx_dma: DotstarSpiTxDma) { - + dotstar_tx_dma: DotstarSpiTxDma, +) { let mut dotstar_spi_config = Config::default(); dotstar_spi_config.frequency = mhz(1); @@ -105,7 +107,8 @@ async fn dotstar_lerp_task( dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, - dotstar_spi_config); + dotstar_spi_config, + ); // let mut dotstar = Apa102::new(dotstar_spi); @@ -115,8 +118,13 @@ async fn dotstar_lerp_task( let mut val = 0; loop { // let _ = dotstar.write([RGB8 { r: val, g: val, b: val }, RGB8 { r: val / 2, g: val / 2, b: val / 2 }].iter().cloned()); - let _ = dotstar_spi.write(&[0x00 as u8, 0x00, 0x00, 0x00, 0xE7, val, 0x00, val, 0xE7, val, 0x00, val, 0xFF, 0xFF, 0xFF, 0xFF]).await; - + let _ = dotstar_spi + .write(&[ + 0x00 as u8, 0x00, 0x00, 0x00, 0xE7, val, 0x00, val, 0xE7, val, 0x00, val, 0xFF, + 0xFF, 0xFF, 0xFF, + ]) + .await; + if counting_up { val += 1; } else { @@ -146,19 +154,20 @@ async fn main(_spawner: Spawner) -> ! { // let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); let _vsw_en = Output::new(p.PE10, Level::High, Speed::Medium); - + info!("kicker startup 1.5!"); let adc = Adc::new(p.ADC1); info!("kicker startup 2!"); - // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { // unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); - unwrap!(spawner.spawn(blink(p.PB15, p.PE0, p.PB9, p.PE1, p.PB5, adc, p.PC3, p.PA1, p.PA2, p.PA3))); + unwrap!(spawner.spawn(blink( + p.PB15, p.PE0, p.PB9, p.PE1, p.PB5, adc, p.PC3, p.PA1, p.PA2, p.PA3 + ))); unwrap!(spawner.spawn(dotstar_lerp_task(p.SPI4, p.PE6, p.PE2, p.DMA2_CH8))); }); -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-breakbeam/main.rs b/kicker-board/src/bin/hwtest-breakbeam/main.rs index 599f36f2..386c4fcc 100644 --- a/kicker-board/src/bin/hwtest-breakbeam/main.rs +++ b/kicker-board/src/bin/hwtest-breakbeam/main.rs @@ -5,13 +5,12 @@ use defmt::*; use ateam_kicker_board::tasks::{get_system_config, ClkSource}; +use embassy_executor::Spawner; use embassy_stm32::gpio::{Level, Output, Speed}; use {defmt_rtt as _, panic_probe as _}; -use embassy_executor::Spawner; -use embassy_time::{Duration, Timer}; use ateam_kicker_board::drivers::breakbeam::Breakbeam; - +use embassy_time::{Duration, Timer}; #[embassy_executor::main] async fn main(_spawner: Spawner) { @@ -23,12 +22,12 @@ async fn main(_spawner: Spawner) { let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); info!("breakbeam startup!"); - + let mut status_led_green = Output::new(p.PE0, Level::Low, Speed::Medium); let mut ball_detected_led1 = Output::new(p.PE2, Level::Low, Speed::Low); let mut ball_detected_led2 = Output::new(p.PE3, Level::Low, Speed::Low); - // Breakbeam + // Breakbeam // nets on schematic are inverted to silkscreen, sorry :/ -Will let mut breakbeam = Breakbeam::new(p.PA1, p.PA0); @@ -42,17 +41,13 @@ async fn main(_spawner: Spawner) { Timer::after(Duration::from_millis(250)).await; breakbeam.enable_tx(); - loop - { + loop { // Enable transmitter, wait 100ms, drive blue status LED if receiving, wait 1 sec // Timer::after(Duration::from_millis(100)).await; - if breakbeam.read() - { + if breakbeam.read() { ball_detected_led1.set_high(); ball_detected_led2.set_high(); - } - else - { + } else { ball_detected_led1.set_low(); ball_detected_led2.set_low(); } @@ -63,14 +58,13 @@ async fn main(_spawner: Spawner) { // if breakbeam.read() // { // status_led_blue.set_high(); - // } + // } // else // { // status_led_blue.set_low(); // } // Timer::after(Duration::from_millis(1000)).await; - - Timer::after(Duration::from_millis(10)).await; + Timer::after(Duration::from_millis(10)).await; } -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-charge/main.rs b/kicker-board/src/bin/hwtest-charge/main.rs index a2df3ccc..ebeb6b6d 100644 --- a/kicker-board/src/bin/hwtest-charge/main.rs +++ b/kicker-board/src/bin/hwtest-charge/main.rs @@ -11,25 +11,26 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ adc::Adc, - gpio::{Level, Output, Speed}, adc::SampleTime, + gpio::{Level, Output, Speed}, }; -use embassy_time::{Duration, Timer, Ticker}; +use embassy_time::{Duration, Ticker, Timer}; use static_cell::StaticCell; -use ateam_kicker_board::*; use ateam_kicker_board::pins::*; +use ateam_kicker_board::*; #[embassy_executor::task] -async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_vsw_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - kick_pin: KickPin) -> ! { - +async fn run_kick( + mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_vsw_pin: PowerRailVswReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + kick_pin: KickPin, +) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); @@ -48,12 +49,19 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, let mut hv = adc.blocking_read(&mut hv_pin) as f32; let mut regv = adc.blocking_read(&mut rail_vsw_pin) as f32; - info!("hv V: {}, vsw reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, vsw reg mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!( + "regulator voltage is below 18.0 ({}), is the battery low or disconnected?", + start_up_battery_voltage + ); warn!("refusing to continue"); loop { reg_charge.set_low(); @@ -61,7 +69,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, kick.set_high(); Timer::after(Duration::from_micros(500)).await; kick.set_low(); - + Timer::after(Duration::from_millis(1000)).await; } } @@ -75,7 +83,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_vsw_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); Timer::after(Duration::from_millis(1000)).await; @@ -86,7 +98,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_vsw_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); reg_charge.set_low(); kick.set_low(); @@ -113,4 +129,4 @@ fn main() -> ! { executor.run(|spawner| { unwrap!(spawner.spawn(run_kick(adc, p.PC3, p.PA1, p.PB15, p.PE0, p.PB9, p.PD9))); }); -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 853b61f7..d1a0c525 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -15,7 +15,7 @@ use embassy_stm32::{ interrupt, interrupt::InterruptExt, pac::Interrupt, - usart::{Config, Parity, StopBits, Uart} + usart::{Config, Parity, StopBits, Uart}, }; use embassy_stm32::{bind_interrupts, peripherals, usart}; @@ -23,13 +23,16 @@ use embassy_time::{Duration, Instant, Ticker, Timer}; use ateam_kicker_board::{ adc_200v_to_rail_voltage, adc_raw_to_v, - kick_manager::{ - KickManager, - KickType}, - pins::*, tasks::{get_system_config, ClkSource}, DEBUG_COMS_UART_QUEUES + kick_manager::{KickManager, KickType}, + pins::*, + tasks::{get_system_config, ClkSource}, + DEBUG_COMS_UART_QUEUES, }; -use ateam_lib_stm32::{idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, + uart::queue::{UartReadQueue, UartWriteQueue}, +}; use ateam_common_packets::bindings::{KickerControl, KickerTelemetry}; @@ -38,20 +41,27 @@ const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; -static_idle_buffered_uart_nl!(COMS, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_COMS_UART_QUEUES); +static_idle_buffered_uart_nl!( + COMS, + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + DEBUG_COMS_UART_QUEUES +); #[embassy_executor::task] async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue, - coms_writer: &'static UartWriteQueue, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - mut rail_pin: PowerRail200vReadPin, - err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin) -> ! { - + coms_reader: &'static UartReadQueue, + coms_writer: &'static UartWriteQueue, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, + charge_pin: ChargePin, + kick_pin: KickPin, + chip_pin: ChipPin, + mut rail_pin: PowerRail200vReadPin, + err_led_pin: RedStatusLedPin, + ball_detected_led_pin: BlueStatusLedPin, +) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); @@ -77,7 +87,10 @@ async fn high_pri_kick_task( let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.blocking_read(&mut vrefint) as f32; - let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.blocking_read(&mut rail_pin) as f32, vrefint_sample)); + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v( + adc.blocking_read(&mut rail_pin) as f32, + vrefint_sample, + )); // optionally pre-flag errors? ///////////////////////////////////// @@ -113,13 +126,27 @@ async fn high_pri_kick_task( telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; // no charge/kick in coms test - let res = kick_manager.command(22.5, rail_voltage, false, KickType::None, 0.0).await; + let res = kick_manager + .command(22.5, rail_voltage, false, KickType::None, 0.0) + .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()); + 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(), + ); kicker_telemetry_packet.rail_voltage = rail_voltage; kicker_telemetry_packet.battery_voltage = 22.5; @@ -162,7 +189,6 @@ async fn high_pri_kick_task( // loop rate control @1KHz ticker.next().await; } - } static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); @@ -213,22 +239,27 @@ async fn main(spawner: Spawner) -> ! { p.DMA2_CH7, p.DMA2_CH2, coms_uart_config, - ).unwrap(); + ) + .unwrap(); COMS_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(spawner, COMS, coms_usart); - - hp_spawner.spawn(high_pri_kick_task( - COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), - COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), - adc, - p.PB15, p.PD9, - p.PD8, p.PC3, - p.PE0, p.PE1)).unwrap(); - + hp_spawner + .spawn(high_pri_kick_task( + COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), + COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), + adc, + p.PB15, + p.PD9, + p.PD8, + p.PC3, + p.PE0, + p.PE1, + )) + .unwrap(); loop { Timer::after_millis(1000).await; } -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-dribbler/main.rs b/kicker-board/src/bin/hwtest-dribbler/main.rs index 815fc97b..88b068e9 100644 --- a/kicker-board/src/bin/hwtest-dribbler/main.rs +++ b/kicker-board/src/bin/hwtest-dribbler/main.rs @@ -8,13 +8,17 @@ use {defmt_rtt as _, panic_probe as _}; use embassy_executor::{InterruptExecutor, Spawner}; use embassy_stm32::{ - bind_interrupts, gpio::Pull, peripherals, usart::{self, Uart} -}; -use embassy_stm32::{ - interrupt, pac::Interrupt + bind_interrupts, + gpio::Pull, + peripherals, + usart::{self, Uart}, }; +use embassy_stm32::{interrupt, pac::Interrupt}; -use ateam_lib_stm32::{drivers::boot::stm32_interface::{self, Stm32Interface}, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface::{self, Stm32Interface}, + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, +}; use ateam_kicker_board::{tasks::get_system_config, *}; @@ -47,22 +51,37 @@ async fn main(_spawner: Spawner) -> ! { info!("kicker startup!"); - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CORDIC, embassy_stm32::interrupt::Priority::P7); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::CORDIC, + embassy_stm32::interrupt::Priority::P7, + ); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CORDIC); - let initial_motor_controller_uart_conifg = stm32_interface::get_bootloader_uart_config(); - let drib_uart = Uart::new(p.USART3, p.PE15, p.PB10, Irqs, p.DMA1_CH1, p.DMA1_CH2, initial_motor_controller_uart_conifg).unwrap(); + let drib_uart = Uart::new( + p.USART3, + p.PE15, + p.PB10, + Irqs, + p.DMA1_CH1, + p.DMA1_CH2, + initial_motor_controller_uart_conifg, + ) + .unwrap(); DRIB_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(uart_queue_spawner, DRIB, drib_uart); let mut drib_motor = Stm32Interface::new_from_pins( &DRIB_IDLE_BUFFERED_UART, - DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PE13, p.PE14, - Pull::None, true); + DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), + DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), + p.PE13, + p.PE14, + Pull::None, + true, + ); defmt::info!("programming firmware image"); @@ -74,4 +93,4 @@ async fn main(_spawner: Spawner) -> ! { } loop {} -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-flash/main.rs b/kicker-board/src/bin/hwtest-flash/main.rs index 7efd150f..cf27f729 100644 --- a/kicker-board/src/bin/hwtest-flash/main.rs +++ b/kicker-board/src/bin/hwtest-flash/main.rs @@ -24,7 +24,6 @@ static FLASH_RX_BUF: StaticCell<[u8; 256]> = StaticCell::new(); #[link_section = ".bss"] static FLASH_TX_BUF: StaticCell<[u8; 256]> = StaticCell::new(); - #[embassy_executor::main] async fn main(_spawner: Spawner) -> ! { let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); @@ -35,12 +34,13 @@ async fn main(_spawner: Spawner) -> ! { let mut spi_config = Config::default(); spi_config.frequency = Hertz(1_000_000); - let spi = Spi::new(p.SPI2, p.PB13, p.PB15, p.PB14, p.DMA1_CH4, p.DMA1_CH3, spi_config); + let spi = Spi::new( + p.SPI2, p.PB13, p.PB15, p.PB14, p.DMA1_CH4, p.DMA1_CH3, spi_config, + ); let rx_buf = FLASH_RX_BUF.init([0; 256]); let tx_buf = FLASH_TX_BUF.init([0; 256]); - let mut flash: AT25DF041B<'static, true> = AT25DF041B::new(spi, p.PB12, rx_buf, tx_buf); let res = flash.verify_chip_id().await; if res.is_err() { @@ -50,4 +50,4 @@ async fn main(_spawner: Spawner) -> ! { } loop {} -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-kick/main.rs b/kicker-board/src/bin/hwtest-kick/main.rs index 41e15ce9..a0ddec78 100644 --- a/kicker-board/src/bin/hwtest-kick/main.rs +++ b/kicker-board/src/bin/hwtest-kick/main.rs @@ -10,26 +10,29 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, opamp::{OpAmp, OpAmpGain, OpAmpSpeed} + adc::{Adc, SampleTime}, + gpio::{Input, Level, Output, Pull, Speed}, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, }; -use embassy_time::{Duration, Timer, Ticker}; +use embassy_time::{Duration, Ticker, Timer}; use static_cell::StaticCell; -use ateam_kicker_board::*; use ateam_kicker_board::pins::*; +use ateam_kicker_board::*; #[embassy_executor::task] -async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - usr_btn_pin: UserBtnPin, - // chip_pin: ChipPin, - kick_pin: KickPin) -> ! { - +async fn run_kick( + mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRailVswReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + usr_btn_pin: UserBtnPin, + // chip_pin: ChipPin, + kick_pin: KickPin, +) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); @@ -50,7 +53,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, let mut hv = adc.blocking_read(&mut hv_pin) as f32; let mut regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, 12v reg mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); let _start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); // if start_up_battery_voltage < 11.5 { @@ -63,7 +70,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, // kick.set_high(); // Timer::after(Duration::from_micros(500)).await; // kick.set_low(); - + // Timer::after(Duration::from_millis(1000)).await; // } // } @@ -86,7 +93,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); Timer::after(Duration::from_millis(1000)).await; @@ -102,7 +113,6 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, Timer::after(Duration::from_millis(1000)).await; - loop { let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.blocking_read(&mut vrefint) as f32; @@ -110,7 +120,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); reg_charge.set_low(); kick.set_low(); @@ -141,9 +155,7 @@ fn main() -> ! { let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { unwrap!(spawner.spawn(run_kick( - adc, - p.PC3, p.PA1, - p.PB15, - p.PE0, p.PB9, p.PB5, p.PD9))); + adc, p.PC3, p.PA1, p.PB15, p.PE0, p.PB9, p.PB5, p.PD9 + ))); }); -} \ No newline at end of file +} diff --git a/kicker-board/src/bin/hwtest-kickstr/main.rs b/kicker-board/src/bin/hwtest-kickstr/main.rs index 81c48bed..591fb131 100644 --- a/kicker-board/src/bin/hwtest-kickstr/main.rs +++ b/kicker-board/src/bin/hwtest-kickstr/main.rs @@ -19,25 +19,27 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, - gpio::{Input, Level, Output, Pull, Speed}, opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + gpio::{Input, Level, Output, Pull, Speed}, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, }; -use embassy_time::{Duration, Timer, Ticker}; +use embassy_time::{Duration, Ticker, Timer}; use static_cell::StaticCell; -use ateam_kicker_board::*; use ateam_kicker_board::pins::*; +use ateam_kicker_board::*; #[embassy_executor::task] -async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - usr_btn_pin: UserBtnPin, - kick_pin: KickPin) -> ! { - +async fn run_kick( + mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRailVswReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + usr_btn_pin: UserBtnPin, + kick_pin: KickPin, +) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); @@ -58,12 +60,19 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, let mut hv = adc.blocking_read(&mut hv_pin) as f32; let mut regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, 12v reg mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!( + "regulator voltage is below 18.0 ({}), is the battery low or disconnected?", + start_up_battery_voltage + ); warn!("refusing to continue"); loop { reg_charge.set_low(); @@ -71,7 +80,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, kick.set_high(); Timer::after(Duration::from_micros(500)).await; kick.set_low(); - + Timer::after(Duration::from_millis(1000)).await; } } @@ -95,14 +104,18 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, reg_charge.set_high(); Timer::after(Duration::from_millis(2000)).await; reg_charge.set_low(); - + let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.blocking_read(&mut vrefint) as f32; hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); + Timer::after(Duration::from_millis(1000)).await; // High = discharge into the solenoid @@ -112,7 +125,6 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, kick.set_low(); Timer::after(Duration::from_millis(1000)).await; - } loop { @@ -122,7 +134,11 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, hv = adc.blocking_read(&mut hv_pin) as f32; regv = adc.blocking_read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + info!( + "hv V: {}, batt mv: {}", + adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample)) + ); reg_charge.set_low(); kick.set_low(); @@ -145,7 +161,6 @@ fn main() -> ! { let mut hv_opamp_inst = OpAmp::new(p.OPAMP3, OpAmpSpeed::HighSpeed); let _hv_opamp = hv_opamp_inst.buffer_ext(p.PB0, p.PB1, OpAmpGain::Mul2); - let mut adc = Adc::new(p.ADC1); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); adc.set_sample_time(SampleTime::CYCLES247_5); @@ -154,9 +169,7 @@ fn main() -> ! { let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { unwrap!(spawner.spawn(run_kick( - adc, - p.PC3, p.PA1, - p.PB15, - p.PE0, p.PB9, p.PB5, p.PD9))); - }); -} \ No newline at end of file + adc, p.PC3, p.PA1, p.PB15, p.PE0, p.PB9, p.PB5, p.PD9 + ))); + }); +} diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 9ec71524..d8a0103e 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -3,32 +3,50 @@ #![feature(type_alias_impl_trait)] #![feature(sync_unsafe_cell)] -use core::sync::atomic::{AtomicBool, AtomicU32}; use core::sync::atomic::Ordering::Relaxed; +use core::sync::atomic::{AtomicBool, AtomicU32}; -use ateam_kicker_board::{drivers::{breakbeam::Breakbeam, DribblerMotor}, include_external_cpp_bin, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin, GreenStatusLedPin}, tasks::{get_system_config, ClkSource}, DEBUG_COMS_UART_QUEUES, DEBUG_DRIB_UART_QUEUES}; +use ateam_kicker_board::{ + drivers::{breakbeam::Breakbeam, DribblerMotor}, + include_external_cpp_bin, + pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin, GreenStatusLedPin}, + tasks::{get_system_config, ClkSource}, + DEBUG_COMS_UART_QUEUES, DEBUG_DRIB_UART_QUEUES, +}; use defmt::*; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::{PubSubChannel, Publisher, Subscriber}}; +use embassy_sync::{ + blocking_mutex::raw::CriticalSectionRawMutex, + pubsub::{PubSubChannel, Publisher, Subscriber}, +}; use {defmt_rtt as _, panic_probe as _}; use libm::{fmaxf, fminf}; use embassy_executor::{InterruptExecutor, Spawner}; use embassy_stm32::{ - adc::{Adc, SampleTime}, bind_interrupts, gpio::{Level, Output, Pull, Speed}, interrupt::{self, InterruptExt}, opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, pac::Interrupt, peripherals, usart::{self, Config, Parity, StopBits, Uart} + adc::{Adc, SampleTime}, + bind_interrupts, + gpio::{Level, Output, Pull, Speed}, + interrupt::{self, InterruptExt}, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + pac::Interrupt, + peripherals, + usart::{self, Config, Parity, StopBits, Uart}, }; use embassy_time::{Duration, Instant, Ticker, Timer}; use ateam_kicker_board::{ - adc_raw_to_v, adc_200v_to_rail_voltage, + adc_200v_to_rail_voltage, adc_raw_to_v, kick_manager::{KickManager, KickType}, - pins::{ - BlueStatusLedPin, ChargePin, ChipPin, PowerRail200vReadPin, KickPin, RedStatusLedPin, - }, + pins::{BlueStatusLedPin, ChargePin, ChipPin, KickPin, PowerRail200vReadPin, RedStatusLedPin}, }; -use ateam_lib_stm32::{drivers::boot::stm32_interface::{get_bootloader_uart_config, Stm32Interface}, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface::{get_bootloader_uart_config, Stm32Interface}, + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, + uart::queue::{UartReadQueue, UartWriteQueue}, +}; use ateam_kicker_board::image_hash::get_kicker_img_hash; @@ -51,7 +69,14 @@ const MAX_TX_PACKET_SIZE: usize = 64; const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 32; const RX_BUF_DEPTH: usize = 3; -static_idle_buffered_uart_nl!(COMS, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_COMS_UART_QUEUES); +static_idle_buffered_uart_nl!( + COMS, + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + DEBUG_COMS_UART_QUEUES +); include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -59,26 +84,25 @@ const DRIB_MAX_TX_PACKET_SIZE: usize = 64; const DRIB_TX_BUF_DEPTH: usize = 3; const DRIB_MAX_RX_PACKET_SIZE: usize = 64; const DRIB_RX_BUF_DEPTH: usize = 20; -static_idle_buffered_uart_nl!(DRIB, DRIB_MAX_RX_PACKET_SIZE, DRIB_RX_BUF_DEPTH, DRIB_MAX_TX_PACKET_SIZE, DRIB_TX_BUF_DEPTH, DEBUG_DRIB_UART_QUEUES); +static_idle_buffered_uart_nl!( + DRIB, + DRIB_MAX_RX_PACKET_SIZE, + DRIB_RX_BUF_DEPTH, + DRIB_MAX_TX_PACKET_SIZE, + DRIB_TX_BUF_DEPTH, + DEBUG_DRIB_UART_QUEUES +); static DRIB_VEL_PUBSUB: PubSubChannel = PubSubChannel::new(); static DRIB_MULT: AtomicU32 = AtomicU32::new(100); static BALL_DETECT: AtomicBool = AtomicBool::new(false); -static DRIB_TELEM_PUBSUB: PubSubChannel = PubSubChannel::new(); - +static DRIB_TELEM_PUBSUB: PubSubChannel = + PubSubChannel::new(); #[embassy_executor::task] async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue< - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, - DEBUG_COMS_UART_QUEUES, - >, - coms_writer: &'static UartWriteQueue< - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, - DEBUG_COMS_UART_QUEUES, - >, + coms_reader: &'static UartReadQueue, + coms_writer: &'static UartWriteQueue, mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, charge_pin: ChargePin, kick_pin: KickPin, @@ -90,7 +114,7 @@ async fn high_pri_kick_task( err_led_pin: RedStatusLedPin, ball_detected_led1_pin: BlueStatusLedPin, drib_vel_pub: Publisher<'static, CriticalSectionRawMutex, f32, 1, 1, 1>, - mut drib_telem_sub: Subscriber<'static, CriticalSectionRawMutex, MotorTelemetry, 1, 1, 1> + mut drib_telem_sub: Subscriber<'static, CriticalSectionRawMutex, MotorTelemetry, 1, 1, 1>, ) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -126,8 +150,7 @@ async fn high_pri_kick_task( let mut shutdown_requested: bool = false; let mut shutdown_completed: bool = false; - let mut rail_voltage_buffer: [f32; RAIL_BUFFER_SIZE] = - [0.0; RAIL_BUFFER_SIZE]; + let mut rail_voltage_buffer: [f32; RAIL_BUFFER_SIZE] = [0.0; RAIL_BUFFER_SIZE]; let mut rail_voltage_filt_indx: usize = 0; // loop rate control @@ -141,7 +164,10 @@ async fn high_pri_kick_task( let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.blocking_read(&mut vrefint); - let rail_voltage_cur = adc_200v_to_rail_voltage(adc_raw_to_v(adc.blocking_read(&mut rail_pin) as f32, vrefint_sample as f32)); + let rail_voltage_cur = adc_200v_to_rail_voltage(adc_raw_to_v( + adc.blocking_read(&mut rail_pin) as f32, + vrefint_sample as f32, + )); // Add new battery read to cyclical buffer. rail_voltage_buffer[rail_voltage_filt_indx] = rail_voltage_cur; @@ -334,7 +360,13 @@ async fn high_pri_kick_task( // if telemetry isn't enabled, the control board doesn't want to talk to us, don't permit any actions let res = if !telemetry_enabled || error_latched { kick_manager - .command(battery_voltage, rail_voltage_ave, false, KickType::None, 0.0) + .command( + battery_voltage, + rail_voltage_ave, + false, + KickType::None, + 0.0, + ) .await } else { if kick_command == KickType::Kick || kick_command == KickType::Chip { @@ -350,7 +382,7 @@ async fn high_pri_kick_task( rail_voltage_ave, charge_hv_rail, kick_command, - kick_speed + kick_speed, ) .await }; @@ -377,7 +409,7 @@ async fn high_pri_kick_task( shutdown_completed as u16, ball_detected as u16, (rail_voltage_ave > CHARGED_THRESH_VOLTAGE) as u16, - Default::default() + Default::default(), ); let charge_pct = rail_voltage_ave / CHARGE_TARGET_VOLTAGE; @@ -386,7 +418,9 @@ async fn high_pri_kick_task( 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]); + 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 { @@ -413,7 +447,6 @@ async fn high_pri_kick_task( if ball_detected { ball_detected_led1.set_high(); - } else { ball_detected_led1.set_low(); } @@ -426,9 +459,17 @@ async fn high_pri_kick_task( #[embassy_executor::task] async fn low_pri_dribble_task( - drib_motor_interface: Stm32Interface<'static, DRIB_MAX_RX_PACKET_SIZE, DRIB_MAX_TX_PACKET_SIZE, DRIB_RX_BUF_DEPTH, DRIB_TX_BUF_DEPTH, DEBUG_DRIB_UART_QUEUES>, - mut drib_vel_sub: Subscriber<'static, CriticalSectionRawMutex, f32, 1, 1, 1>, - drib_telem_pub: Publisher<'static, CriticalSectionRawMutex, MotorTelemetry, 1, 1, 1>) -> ! { + drib_motor_interface: Stm32Interface< + 'static, + DRIB_MAX_RX_PACKET_SIZE, + DRIB_MAX_TX_PACKET_SIZE, + DRIB_RX_BUF_DEPTH, + DRIB_TX_BUF_DEPTH, + DEBUG_DRIB_UART_QUEUES, + >, + mut drib_vel_sub: Subscriber<'static, CriticalSectionRawMutex, f32, 1, 1, 1>, + drib_telem_pub: Publisher<'static, CriticalSectionRawMutex, MotorTelemetry, 1, 1, 1>, +) -> ! { let mut drib_motor = DribblerMotor::new(drib_motor_interface, DRIB_FW_IMG, 1.0); defmt::info!("flashing dribbler motor firmware..."); @@ -502,7 +543,6 @@ bind_interrupts!(struct Irqs { USART3 => usart::InterruptHandler; }); - #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { let stm32_config = get_system_config(ClkSource::InternalOscillator); @@ -537,18 +577,26 @@ async fn main(spawner: Spawner) -> ! { // spawn the task at the highest prio unwrap!(hp_spawner.spawn(high_pri_kick_task( - COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), + COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), + COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), adc, - p.PB15, p.PD9, p.PD8, - p.PC2, p.PC0, + p.PB15, + p.PD9, + p.PD8, + p.PC2, + p.PC0, p.PC3, - p.PB9, p.PE0, + p.PB9, + p.PE0, p.PE1, - DRIB_VEL_PUBSUB.publisher().expect("failed to get dribbler vel publisher for kick task"), - DRIB_TELEM_PUBSUB.subscriber().expect("failed to get drib telem sub for kick task") + DRIB_VEL_PUBSUB + .publisher() + .expect("failed to get dribbler vel publisher for kick task"), + DRIB_TELEM_PUBSUB + .subscriber() + .expect("failed to get drib telem sub for kick task") ))); - ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // ////////////////////////////////// @@ -566,7 +614,8 @@ async fn main(spawner: Spawner) -> ! { p.DMA2_CH7, p.DMA2_CH2, coms_uart_config, - ).unwrap(); + ) + .unwrap(); COMS_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(mp_spawner, COMS, coms_usart); @@ -578,26 +627,39 @@ async fn main(spawner: Spawner) -> ! { let initial_motor_controller_uart_config = get_bootloader_uart_config(); let drib_uart = Uart::new( p.USART3, - p.PE15, p.PB10, + p.PE15, + p.PB10, Irqs, - p.DMA1_CH1, p.DMA1_CH2, - initial_motor_controller_uart_config - ).unwrap(); + p.DMA1_CH1, + p.DMA1_CH2, + initial_motor_controller_uart_config, + ) + .unwrap(); DRIB_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(mp_spawner, DRIB, drib_uart); let drib_motor_interface = Stm32Interface::new_from_pins( &DRIB_IDLE_BUFFERED_UART, - DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PE13, p.PE14, - Pull::None, true); - - spawner.spawn(low_pri_dribble_task( - drib_motor_interface, - DRIB_VEL_PUBSUB.subscriber().expect("failed to get dribler vel subscriber for drib task"), - DRIB_TELEM_PUBSUB.publisher().expect("failed to get drib telem pub for dribble task"), - )).expect("failed to spawn dribble task"); + DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), + DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), + p.PE13, + p.PE14, + Pull::None, + true, + ); + + spawner + .spawn(low_pri_dribble_task( + drib_motor_interface, + DRIB_VEL_PUBSUB + .subscriber() + .expect("failed to get dribler vel subscriber for drib task"), + DRIB_TELEM_PUBSUB + .publisher() + .expect("failed to get drib telem pub for dribble task"), + )) + .expect("failed to spawn dribble task"); ///////////////////////////////////////// // spin and allow other tasks to run // diff --git a/kicker-board/src/drivers/breakbeam.rs b/kicker-board/src/drivers/breakbeam.rs index a109ace3..43a03f6c 100644 --- a/kicker-board/src/drivers/breakbeam.rs +++ b/kicker-board/src/drivers/breakbeam.rs @@ -1,4 +1,7 @@ -use embassy_stm32::{gpio::{Input, Level, Output, Pin, Pull, Speed}, Peripheral}; +use embassy_stm32::{ + gpio::{Input, Level, Output, Pin, Pull, Speed}, + Peripheral, +}; pub struct Breakbeam<'a> { pin_tx: Output<'a>, @@ -6,13 +9,13 @@ pub struct Breakbeam<'a> { } impl<'a> Breakbeam<'a> { - pub fn new(pin_tx: impl Peripheral

+ 'a, pin_rx: impl Peripheral

+ 'a) -> Self { + pub fn new( + pin_tx: impl Peripheral

+ 'a, + pin_rx: impl Peripheral

+ 'a, + ) -> Self { let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); let pin_rx = Input::new(pin_rx, Pull::Down); - Self { - pin_tx, - pin_rx - } + Self { pin_tx, pin_rx } } #[inline] diff --git a/kicker-board/src/drivers/mod.rs b/kicker-board/src/drivers/mod.rs index 0b5171ee..cb7b7477 100644 --- a/kicker-board/src/drivers/mod.rs +++ b/kicker-board/src/drivers/mod.rs @@ -1,8 +1,21 @@ use core::mem::MaybeUninit; -use ateam_common_packets::bindings::{MotionCommandType::{self, OPEN_LOOP}, MotorCommandPacket, MotorCommandType::{MCP_MOTION, MCP_PARAMS}, MotorResponse, MotorResponseType::{MRP_MOTION, MRP_PARAMS}, MotorTelemetry, ParameterMotorResponse}; -use ateam_lib_stm32::{drivers::boot::stm32_interface::Stm32Interface, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; -use embassy_stm32::{gpio::{Pin, Pull}, usart::Parity}; +use ateam_common_packets::bindings::{ + MotionCommandType::{self, OPEN_LOOP}, + MotorCommandPacket, + MotorCommandType::{MCP_MOTION, MCP_PARAMS}, + MotorResponse, + MotorResponseType::{MRP_MOTION, MRP_PARAMS}, + MotorTelemetry, ParameterMotorResponse, +}; +use ateam_lib_stm32::{ + drivers::boot::stm32_interface::Stm32Interface, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; +use embassy_stm32::{ + gpio::{Pin, Pull}, + usart::Parity, +}; use embassy_time::{with_timeout, Duration, Timer}; use crate::image_hash; @@ -18,14 +31,8 @@ pub struct DribblerMotor< const DEPTH_RX: usize, const DEPTH_TX: usize, > { - stm32_uart_interface: Stm32Interface< - 'a, - LEN_RX, - LEN_TX, - DEPTH_RX, - DEPTH_TX, - DEBUG_DRIB_UART_QUEUES - >, + stm32_uart_interface: + Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_DRIB_UART_QUEUES>, firmware_image: &'a [u8], current_state: MotorTelemetry, @@ -54,14 +61,12 @@ impl< LEN_TX, DEPTH_RX, DEPTH_TX, - DEBUG_DRIB_UART_QUEUES + DEBUG_DRIB_UART_QUEUES, >, firmware_image: &'a [u8], ball_detected_thresh: f32, - ) -> DribblerMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> - { - let start_state: MotorTelemetry = - { unsafe { MaybeUninit::zeroed().assume_init() } }; + ) -> DribblerMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { + let start_state: MotorTelemetry = { unsafe { MaybeUninit::zeroed().assume_init() } }; let start_params_state: ParameterMotorResponse = { unsafe { MaybeUninit::zeroed().assume_init() } }; @@ -90,13 +95,19 @@ impl< reset_pin: impl Pin, firmware_image: &'a [u8], ball_detected_thresh: f32, - ) -> DribblerMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> - { + ) -> DribblerMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // Need a Pull None to allow for STSPIN watchdog usage. - let stm32_interface = Stm32Interface::new_from_pins(uart, read_queue, write_queue, boot0_pin, reset_pin, Pull::None, false); - - let start_state: MotorTelemetry = - { unsafe { MaybeUninit::zeroed().assume_init() } }; + let stm32_interface = Stm32Interface::new_from_pins( + uart, + read_queue, + write_queue, + boot0_pin, + reset_pin, + Pull::None, + false, + ); + + let start_state: MotorTelemetry = { unsafe { MaybeUninit::zeroed().assume_init() } }; let start_params_state: ParameterMotorResponse = { unsafe { MaybeUninit::zeroed().assume_init() } }; @@ -149,8 +160,11 @@ impl< if self.current_params_state.firmware_img_hash != [0; 4] { let current_img_hash = self.current_params_state.firmware_img_hash; defmt::debug!("Dribbler Interface - Received parameter response"); - defmt::trace!("Dribbler Interface - Current device image hash {:x}", current_img_hash); - return current_img_hash + defmt::trace!( + "Dribbler Interface - Current device image hash {:x}", + current_img_hash + ); + return current_img_hash; }; Timer::after(Duration::from_millis(5)).await; @@ -159,10 +173,15 @@ impl< pub async fn check_device_has_latest_default_image(&mut self) -> Result { let latest_img_hash = self.get_latest_default_img_hash(); - defmt::debug!("Dribbler Interface - Latest default image hash - {:x}", latest_img_hash); + defmt::debug!( + "Dribbler Interface - Latest default image hash - {:x}", + latest_img_hash + ); defmt::trace!("Dribbler Interface - Update UART config 2 MHz"); - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; let res; @@ -174,14 +193,16 @@ impl< defmt::trace!("Dribbler Interface - Device has the latest default image"); res = Ok(true); } else { - defmt::trace!("Dribbler Interface - Device does not have the latest default image"); + defmt::trace!( + "Dribbler Interface - Device does not have the latest default image" + ); res = Ok(false); } - }, + } Err(_) => { defmt::debug!("Dribbler Interface - No device response, image hash unknown"); res = Err(()); - }, + } } // Make sure that the uart queue is empty of any possible parameter // response packets, which may cause side effects for the flashing @@ -190,15 +211,23 @@ impl< return res; } - pub async fn init_firmware_image(&mut self, flash: bool, fw_image_bytes: &[u8]) -> Result<(), ()> { + pub async fn init_firmware_image( + &mut self, + flash: bool, + fw_image_bytes: &[u8], + ) -> Result<(), ()> { if flash { defmt::info!("Dribbler Interface - Flashing firmware image"); - self.stm32_uart_interface.load_firmware_image(fw_image_bytes, true).await?; + self.stm32_uart_interface + .load_firmware_image(fw_image_bytes, true) + .await?; } else { defmt::info!("Dribbler Interface - Skipping firmware flash"); } - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; + self.stm32_uart_interface + .update_uart_config(2_000_000, Parity::ParityEven) + .await; Timer::after(Duration::from_millis(1)).await; @@ -224,7 +253,7 @@ impl< } else { flash = true; } - }, + } Err(_) => { flash = true; } @@ -238,7 +267,12 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("Dribbler - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); + defmt::warn!( + "Dribbler - Got invalid packet of len {:?} (expected {:?}) data: {:?}", + buf.len(), + core::mem::size_of::(), + buf + ); continue; } @@ -286,7 +320,6 @@ impl< // info!("reset_watchdog_window {:?}", mrp.data.motion.reset_watchdog_window()); // info!("reset_low_power {:?}", mrp.data.motion.reset_low_power()); // info!("reset_software {:?}", mrp.data.motion.reset_software()); - } else if mrp.type_ == MRP_PARAMS { self.current_params_state = mrp.data.params; } @@ -344,7 +377,9 @@ impl< cmd.type_ = MCP_MOTION; cmd.crc32 = 0; cmd.data.motion.set_reset(self.reset_flagged as u32); - cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32); + cmd.data + .motion + .set_enable_telemetry(self.telemetry_enabled as u32); cmd.data.motion.motion_control_type = self.motion_type; cmd.data.motion.setpoint = self.setpoint; @@ -380,7 +415,9 @@ impl< } pub fn check_hall_error(&self) -> bool { - self.current_state.hall_power_error() != 0 || self.current_state.hall_disconnected_error() != 0 || self.current_state.hall_enc_vel_disagreement_error() != 0 + self.current_state.hall_power_error() != 0 + || self.current_state.hall_disconnected_error() != 0 + || self.current_state.hall_enc_vel_disagreement_error() != 0 } pub fn read_current(&self) -> f32 { diff --git a/kicker-board/src/image_hash.rs b/kicker-board/src/image_hash.rs index 04d67d67..1a919c0a 100644 --- a/kicker-board/src/image_hash.rs +++ b/kicker-board/src/image_hash.rs @@ -2,20 +2,20 @@ use core::ptr::read_volatile; struct ImgHash { #[allow(dead_code)] - img_hash_magic: [u8; 16], // Used to find this struct in binary - img_hash: [u8; 16] // Hash is injected after compilation + img_hash_magic: [u8; 16], // Used to find this struct in binary + img_hash: [u8; 16], // Hash is injected after compilation } /// Dribbler image hash, saved in the kicker image static mut DRIBBLER_IMG_HASH: ImgHash = ImgHash { img_hash_magic: *b"DrblrImgHashKick", - img_hash: [0; 16] + img_hash: [0; 16], }; /// Kicker image hash, saved in the kicker image static mut KICKER_IMG_HASH: ImgHash = ImgHash { img_hash_magic: *b"KickrImgHashKick", - img_hash: [0; 16] + img_hash: [0; 16], }; pub fn get_dribbler_img_hash() -> [u8; 16] { diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index 0775e4a3..a91aef0f 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -1,8 +1,8 @@ /* * This file is responsible for managing the mechanically and - * electrically critical sections of operation. Foundational + * electrically critical sections of operation. Foundational * assumptions and requirements are listed below. - * + * * ASSUMPTIONS: * 1. The init code hands off to the manager in a default intert state * (no charge commanded, no discharge commanded on any channel). @@ -10,11 +10,11 @@ * this file. * 3. The Rust type ownership system prevents "safe" external access of * the critical hardware interface. - * 4. Charging (active or in soft stop) during any discharge event is + * 4. Charging (active or in soft stop) during any discharge event is * unsafe behavior. * 5. Discharging on multiple channels at once is unsafe. * 6. All async call backs run at the highest priority. - * + * * HYPOTHESES: * 1. The charge pin is not in an active state when kick discharge pin * is in an active state. @@ -22,28 +22,27 @@ * is in an active state. */ +use ateam_lib_stm32::math::range::Range; use embassy_stm32::gpio::Output; use embassy_time::{Duration, Timer}; -use ateam_lib_stm32::math::range::Range; const MIN_KICK_DURATION_US: f32 = 1400.0; -const MAX_KICK_DURATION_US: f32 = 5500.0; // 10ms (10k us) max power kick -const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick +const MAX_KICK_DURATION_US: f32 = 5500.0; // 10ms (10k us) max power kick +const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick -const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off -const KICK_COOLDOWN: Duration = Duration::from_millis(100); // TODO: get estiamted mechanical return time from Matt and pad it -const CHIP_COOLDOWN: Duration = Duration::from_millis(250); // TODO: get estiamted mechanical return time from Matt and pad it +const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off +const KICK_COOLDOWN: Duration = Duration::from_millis(100); // TODO: get estiamted mechanical return time from Matt and pad it +const CHIP_COOLDOWN: Duration = Duration::from_millis(250); // TODO: get estiamted mechanical return time from Matt and pad it -const MAX_SAFE_RAIL_VOLTAGE: f32 = 195.0; // rail is rated for 200V, and should stop charging around 180V +const MAX_SAFE_RAIL_VOLTAGE: f32 = 195.0; // rail is rated for 200V, and should stop charging around 180V const VBATT_OVERVOLTAGE_LOCKOUT: f32 = 27.2; const VBATT_UNDERVOLTAGE_LOCKOUT: f32 = 17.2; - #[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord)] pub enum KickType { Kick, Chip, - None + None, } pub struct KickManager<'a> { @@ -58,19 +57,26 @@ pub struct KickManager<'a> { impl<'a> KickManager<'a> { pub fn new( - charge_pin: Output<'a>, - kick_pin: Output<'a>, - chip_pin: Output<'a>, + charge_pin: Output<'a>, + kick_pin: Output<'a>, + chip_pin: Output<'a>, ) -> KickManager<'a> { KickManager { charge_pin, kick_pin, chip_pin, - error_latched: false + error_latched: false, } } - pub async fn command(&mut self, battery_voltage: f32, rail_voltage:f32, charge: bool, kick_type: KickType, kick_speed: f32) -> Result<(), ()> { + pub async fn command( + &mut self, + battery_voltage: f32, + rail_voltage: f32, + charge: bool, + kick_type: KickType, + kick_speed: f32, + ) -> Result<(), ()> { // latch an error for invalid battery voltage if !(VBATT_UNDERVOLTAGE_LOCKOUT..=VBATT_OVERVOLTAGE_LOCKOUT).contains(&battery_voltage) { self.error_latched = true; @@ -93,7 +99,8 @@ impl<'a> KickManager<'a> { // set charge duration via mapping from kick speed let kick_speed_map = Range::new(0f32, 5.3f32); let kick_duration_map = Range::new(MIN_KICK_DURATION_US, MAX_KICK_DURATION_US); - let charge_duration_us: f32 = kick_speed_map.map_value_to_range(kick_speed, &kick_duration_map); + let charge_duration_us: f32 = + kick_speed_map.map_value_to_range(kick_speed, &kick_duration_map); // handle charge, kick, and chip match kick_type { @@ -121,7 +128,7 @@ impl<'a> KickManager<'a> { // cooldown for mechanical return of the subsystem Timer::after(KICK_COOLDOWN).await; - }, + } KickType::Chip => { // disable kick and chip self.kick_pin.set_low(); @@ -133,7 +140,10 @@ impl<'a> KickManager<'a> { // begin chip, wait time to determine power self.chip_pin.set_high(); - Timer::after(Duration::from_micros((MAX_CHIP_DURATION_US * kick_speed) as u64)).await; + Timer::after(Duration::from_micros( + (MAX_CHIP_DURATION_US * kick_speed) as u64, + )) + .await; // end chip self.chip_pin.set_low(); @@ -143,7 +153,7 @@ impl<'a> KickManager<'a> { if charge { self.charge_pin.set_high(); } - + // cooldown for mechanical return of the subsystem Timer::after(CHIP_COOLDOWN).await; } @@ -158,7 +168,7 @@ impl<'a> KickManager<'a> { } else { self.charge_pin.set_low(); } - } + } } Ok(()) @@ -167,4 +177,4 @@ impl<'a> KickManager<'a> { pub fn clear_error(&mut self) { self.error_latched = false; } -} \ No newline at end of file +} diff --git a/kicker-board/src/lib.rs b/kicker-board/src/lib.rs index 107e1d1f..682ee574 100644 --- a/kicker-board/src/lib.rs +++ b/kicker-board/src/lib.rs @@ -7,9 +7,9 @@ pub mod drivers; pub mod tasks; +pub mod image_hash; pub mod kick_manager; pub mod pins; -pub mod image_hash; // pub mod queue; // pub mod uart_queue; @@ -19,9 +19,17 @@ pub const DEBUG_DRIB_UART_QUEUES: bool = false; #[macro_export] macro_rules! include_external_cpp_bin { ($var_name:ident, $bin_file:literal) => { - pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../motor-controller/build/bin/", $bin_file)).len()] - = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../motor-controller/build/bin/", $bin_file)); - } + pub static $var_name: &[u8; include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../motor-controller/build/bin/", + $bin_file + )) + .len()] = include_bytes!(concat!( + env!("CARGO_MANIFEST_DIR"), + "/../motor-controller/build/bin/", + $bin_file + )); + }; } pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV @@ -43,9 +51,9 @@ pub const fn adc_12v_to_rail_voltage(adc_mv: f32) -> f32 { } pub const fn adc_5v0_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv / 2.0 * 5.0 + adc_mv / 2.0 * 5.0 } pub const fn adc_3v3_to_rail_voltage(adc_mv: f32) -> f32 { adc_mv / 2.0 * 3.3 -} \ No newline at end of file +} diff --git a/kicker-board/src/pins.rs b/kicker-board/src/pins.rs index 7753e738..55293404 100644 --- a/kicker-board/src/pins.rs +++ b/kicker-board/src/pins.rs @@ -8,7 +8,6 @@ pub type ChargePin = PB15; pub type KickPin = PD9; pub type ChipPin = PD8; - //////////////////////////// // Voltage Measurements // //////////////////////////// @@ -21,7 +20,6 @@ pub type PowerRail5v0ReadPin = PA2; pub type PowerRail3v3ReadPin = PA3; pub type PowerRailAdcDma = DMA1_CH7; - ////////////////////// // Breakbeam Pins // ////////////////////// @@ -46,7 +44,6 @@ pub type BreakbeamRightIntPin = PD10; pub type BreakbeamRightI2cRxDma = DMA2_CH3; pub type BreakbeamRightI2cTxDma = DMA2_CH4; - /////////////// // User IO // /////////////// @@ -72,8 +69,7 @@ pub type TrimPot0Pin = PE8; pub type TrimPot1Pin = PE9; pub type SwdSwclkPin = PA14; -pub type SwdSwdioPin = PA13; - +pub type SwdSwdioPin = PA13; ////////////////////////////// // Control Communications // @@ -87,7 +83,6 @@ pub type ComsUartRtsPin = PA12; pub type ComsUartTxDma = DMA1_CH1; pub type ComsUartRxDma = DMA1_CH2; - /////////////////// // Peripherals // /////////////////// @@ -119,4 +114,3 @@ pub type DribblerUartRxDma = DMA1_CH5; pub type DribblerUartTxDma = DMA1_CH6; pub type TempProbeReadPin = PC4; - diff --git a/kicker-board/src/tasks/mod.rs b/kicker-board/src/tasks/mod.rs index 4492b4fb..4973db37 100644 --- a/kicker-board/src/tasks/mod.rs +++ b/kicker-board/src/tasks/mod.rs @@ -1,4 +1,11 @@ -use embassy_stm32::{rcc::{mux::Adcsel, AHBPrescaler, APBPrescaler, Hse, HseMode, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllRDiv, PllSource, Sysclk}, time::Hertz, Config}; +use embassy_stm32::{ + rcc::{ + mux::Adcsel, AHBPrescaler, APBPrescaler, Hse, HseMode, Pll, PllMul, PllPDiv, PllPreDiv, + PllQDiv, PllRDiv, PllSource, Sysclk, + }, + time::Hertz, + Config, +}; #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum ClkSource { @@ -20,7 +27,7 @@ pub fn get_system_config(clk_src: ClkSource) -> Config { config.rcc.pll = Some(Pll { source: PllSource::HSE, prediv: PllPreDiv::DIV2, // root frequency to PLL will be 4MHz after pre_div regardless of source - mul: PllMul::MUL85, // multiply up by 85 to get 340 MHz + mul: PllMul::MUL85, // multiply up by 85 to get 340 MHz divp: Some(PllPDiv::DIV2), // 340 MHz / 2 = 170 MHz p which is feeds sysclk divq: Some(PllQDiv::DIV2), // 340 MHz / 2 = 170 MHz divr: Some(PllRDiv::DIV2), // 340 MHz / 2 = 170 MHz @@ -30,7 +37,7 @@ pub fn get_system_config(clk_src: ClkSource) -> Config { config.rcc.pll = Some(Pll { source: PllSource::HSI, prediv: PllPreDiv::DIV4, // root frequency to PLL will be 4MHz after pre_div regardless of source - mul: PllMul::MUL85, // multiply up by 85 to get 340 MHz + mul: PllMul::MUL85, // multiply up by 85 to get 340 MHz divp: Some(PllPDiv::DIV2), // 340 MHz / 2 = 170MHz p which is feeds sysclk divq: Some(PllQDiv::DIV2), // 340 MHz / 2 = 170MHz divr: Some(PllRDiv::DIV2), // 340 MHz / 2 = 170MHz @@ -49,4 +56,4 @@ pub fn get_system_config(clk_src: ClkSource) -> Config { config.rcc.sys = Sysclk::PLL1_R; config -} \ No newline at end of file +} diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs index da4473c6..c1514f9b 100644 --- a/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs @@ -4,13 +4,12 @@ use ateam_lib_stm32::drivers::switches::button::{AdvExtiButton, ADV_BTN_EVENT_DOUBLE_TAP}; -use defmt_rtt as _; +use defmt_rtt as _; use embassy_time::Timer; use panic_probe as _; - #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) -> !{ +async fn main(_spawner: embassy_executor::Spawner) -> ! { // this actually gets us 64MHz peripheral bus clock let stm32_config: embassy_stm32::Config = Default::default(); let p = embassy_stm32::init(stm32_config); @@ -23,4 +22,4 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ Timer::after_millis(10).await; } -} \ No newline at end of file +} diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs index 9fb00795..e39942df 100644 --- a/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs @@ -4,13 +4,12 @@ use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; -use defmt_rtt as _; +use defmt_rtt as _; use embassy_time::Timer; use panic_probe as _; - #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) -> !{ +async fn main(_spawner: embassy_executor::Spawner) -> ! { // this actually gets us 64MHz peripheral bus clock let stm32_config: embassy_stm32::Config = Default::default(); let p = embassy_stm32::init(stm32_config); @@ -24,4 +23,4 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ Timer::after_millis(10).await; } -} \ No newline at end of file +} diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index 42801285..22327119 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -4,20 +4,28 @@ use core::sync::atomic::AtomicU32; +use embassy_executor::{Executor, InterruptExecutor}; use embassy_stm32::{ - bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, peripherals::{self, *}, usart::{self, *} + bind_interrupts, + exti::ExtiInput, + gpio::{Level, Output, Pull, Speed}, + interrupt, + peripherals::{self, *}, + usart::{self, *}, }; -use embassy_executor::{Executor, InterruptExecutor}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; use defmt::*; -use defmt_rtt as _; +use defmt_rtt as _; use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; type LedGreenPin = PB0; type LedYellowPin = PE1; @@ -41,7 +49,7 @@ struct LockTest { impl LockTest { const fn new() -> Self { LockTest { - lock: Mutex::new(false) + lock: Mutex::new(false), } } @@ -62,11 +70,13 @@ struct StupidPacket { } #[embassy_executor::task] -async fn rx_task(coms_reader: &'static UartReadQueue) { +async fn rx_task( + coms_reader: &'static UartReadQueue, +) { let mut rx_packet: StupidPacket = StupidPacket { - fields_of_minimal_intelligence: [0x55AA55AA; 16] + fields_of_minimal_intelligence: [0x55AA55AA; 16], }; - + loop { while let Ok(res) = coms_reader.try_dequeue() { let buf = res.data(); @@ -80,9 +90,13 @@ async fn rx_task(coms_reader: &'static UartReadQueue()) { + for (i, val) in buf + .iter() + .enumerate() + .take(core::mem::size_of::()) + { *state.add(i) = *val; - } + } } defmt::info!("got a packet"); @@ -93,9 +107,11 @@ async fn rx_task(coms_reader: &'static UartReadQueue) { +async fn tx_task( + coms_writer: &'static UartWriteQueue, +) { let tx_packet: StupidPacket = StupidPacket { - fields_of_minimal_intelligence: [0x55AA55AA; 16] + fields_of_minimal_intelligence: [0x55AA55AA; 16], }; loop { @@ -116,13 +132,20 @@ async fn tx_task(coms_writer: &'static UartWriteQueue) { - + coms_writer: &'static IdleBufferedUart< + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + DEBUG_QUEUE, + >, +) { let mut usr_btn = ExtiInput::new(usr_btn_pin, usr_btn_exti, Pull::Down); let mut green_led = Output::new(led_green_pin, Level::High, Speed::Medium); @@ -181,15 +204,18 @@ bind_interrupts!(struct Irqs { }); #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) -> !{ +async fn main(_spawner: embassy_executor::Spawner) -> ! { // this actually gets us 64MHz peripheral bus clock let stm32_config: embassy_stm32::Config = Default::default(); let p = embassy_stm32::init(stm32_config); - + // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::TIM2, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::TIM2, + embassy_stm32::interrupt::Priority::P6, + ); // let high_pri_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); ////////////////////////////////// @@ -209,7 +235,8 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ p.DMA1_CH1, p.DMA1_CH2, coms_uart_config, - ).unwrap(); + ) + .unwrap(); LT.lock_test(); @@ -221,10 +248,21 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14, &COMS_IDLE_BUFFERED_UART))); + unwrap!(spawner.spawn(handle_btn_press( + p.PC13, + p.EXTI13, + p.PB0, + p.PE1, + p.PB14, + &COMS_IDLE_BUFFERED_UART + ))); unwrap!(spawner.spawn(rx_task(COMS_IDLE_BUFFERED_UART.get_uart_read_queue()))); unwrap!(spawner.spawn(tx_task(COMS_IDLE_BUFFERED_UART.get_uart_write_queue()))); - spawner.spawn(idle_buffered_uart_read_task!(coms, coms_uart_rx)).unwrap(); - spawner.spawn(idle_buffered_uart_write_task!(coms, coms_uart_tx)).unwrap(); + spawner + .spawn(idle_buffered_uart_read_task!(coms, coms_uart_rx)) + .unwrap(); + spawner + .spawn(idle_buffered_uart_write_task!(coms, coms_uart_tx)) + .unwrap(); }); -} \ No newline at end of file +} diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index a4743e0d..7da013b4 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -4,6 +4,7 @@ use core::sync::atomic::AtomicU16; +use embassy_executor::{Executor, InterruptExecutor}; use embassy_stm32::{ bind_interrupts, exti::ExtiInput, @@ -11,18 +12,20 @@ use embassy_stm32::{ interrupt, pac::Interrupt, peripherals::{self, *}, - usart::{self, *} + usart::{self, *}, }; -use embassy_executor::{Executor, InterruptExecutor}; use embassy_time::Timer; use defmt::*; -use defmt_rtt as _; +use defmt_rtt as _; use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{ + idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, + uart::queue::{UartReadQueue, UartWriteQueue}, +}; type LedGreenPin = PB0; type LedYellowPin = PE1; @@ -47,11 +50,13 @@ struct StupidPacket { } #[embassy_executor::task] -async fn rx_task(coms_reader: &'static UartReadQueue) { +async fn rx_task( + coms_reader: &'static UartReadQueue, +) { let mut rx_packet: StupidPacket = StupidPacket { - fields_of_minimal_intelligence: [0x55AA55AA; 16] + fields_of_minimal_intelligence: [0x55AA55AA; 16], }; - + loop { while let Ok(res) = coms_reader.try_dequeue() { let buf = res.data(); @@ -65,9 +70,13 @@ async fn rx_task(coms_reader: &'static UartReadQueue()) { + for (i, val) in buf + .iter() + .enumerate() + .take(core::mem::size_of::()) + { *state.add(i) = *val; - } + } } } @@ -77,9 +86,11 @@ async fn rx_task(coms_reader: &'static UartReadQueue) { +async fn tx_task( + coms_writer: &'static UartWriteQueue, +) { let tx_packet: StupidPacket = StupidPacket { - fields_of_minimal_intelligence: [0x55AA55AA; 16] + fields_of_minimal_intelligence: [0x55AA55AA; 16], }; loop { @@ -101,19 +112,19 @@ async fn tx_task(coms_writer: &'static UartWriteQueue !{ +async fn main(_spawner: embassy_executor::Spawner) -> ! { // this actually gets us 64MHz peripheral bus clock let stm32_config: embassy_stm32::Config = Default::default(); let p = embassy_stm32::init(stm32_config); - + // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::TIM2, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority( + embassy_stm32::interrupt::TIM2, + embassy_stm32::interrupt::Priority::P6, + ); let _high_pri_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); ////////////////////////////////// @@ -181,7 +195,8 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ p.DMA1_CH1, p.DMA1_CH2, coms_uart_config, - ).unwrap(); + ) + .unwrap(); let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); @@ -194,7 +209,11 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14))); unwrap!(spawner.spawn(rx_task(COMS_IDLE_BUFFERED_UART.get_uart_read_queue()))); unwrap!(spawner.spawn(tx_task(COMS_IDLE_BUFFERED_UART.get_uart_write_queue()))); - spawner.spawn(idle_buffered_uart_read_task!(coms, coms_uart_rx)).unwrap(); - spawner.spawn(idle_buffered_uart_write_task!(coms, coms_uart_tx)).unwrap(); + spawner + .spawn(idle_buffered_uart_read_task!(coms, coms_uart_rx)) + .unwrap(); + spawner + .spawn(idle_buffered_uart_write_task!(coms, coms_uart_tx)) + .unwrap(); }); -} \ No newline at end of file +} diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index ba93e9dc..55ab629d 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -1,9 +1,11 @@ use std::path::Path; -fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) -> Vec<(String, u16)>{ +fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) -> Vec<(String, u16)> { // (Ref pitch is assumed to be A4) let mut pitches = Vec::<(String, u16)>::new(); - let scale : [&str; 12] = ["a", "as", "b", "c", "cs", "d", "ds", "e", "f", "fs", "g", "gs"]; + let scale: [&str; 12] = [ + "a", "as", "b", "c", "cs", "d", "ds", "e", "f", "fs", "g", "gs", + ]; // Calculate ALL the pitches! for o in 1..octaves_below { for semitone in 0..11 { @@ -28,46 +30,56 @@ fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) -> Ve pitches } -fn write_pitches_to_file(pitch_set: Vec<(String, u16)>){ +fn write_pitches_to_file(pitch_set: Vec<(String, u16)>) { // I think this matters as Git will convert line endings, so we - // don't want to invalidate the cache because our + // don't want to invalidate the cache because our #[cfg(windows)] const LINE_ENDING: &'static str = "\r\n"; #[cfg(not(windows))] const LINE_ENDING: &str = "\n"; - + let mut generated_file_contents = String::new(); generated_file_contents.push_str("// This file is autogenerated by build.rs"); generated_file_contents.push_str(LINE_ENDING); generated_file_contents.push_str(LINE_ENDING); for pitch in pitch_set { // Write each pitch name as a constant - // in the format PITCH_NAME_OCTAVE, ex. + // in the format PITCH_NAME_OCTAVE, ex. // pub const A4: u16 = 440 - generated_file_contents.push_str(format!("pub const {}: u16 = {};{}", pitch.0.to_uppercase(), pitch.1, LINE_ENDING).as_str()); + generated_file_contents.push_str( + format!( + "pub const {}: u16 = {};{}", + pitch.0.to_uppercase(), + pitch.1, + LINE_ENDING + ) + .as_str(), + ); } - + let pitches_file = Path::new("./src/audio/pitches.rs"); let write_contents: bool = if !pitches_file.exists() { true } else { - let file_contents = std::fs::read_to_string(pitches_file).expect("failed to open existing pitches file"); + let file_contents = + std::fs::read_to_string(pitches_file).expect("failed to open existing pitches file"); file_contents != generated_file_contents }; if write_contents { - std::fs::write(pitches_file, generated_file_contents).expect("failed to write pitches file contents"); + std::fs::write(pitches_file, generated_file_contents) + .expect("failed to write pitches file contents"); } } -fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ +fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16 { let diff = semitones as f32 / 12_f32; - (ref_pitch as f32/ 2_f32.powf(diff)) as u16 + (ref_pitch as f32 / 2_f32.powf(diff)) as u16 } fn main() { let a4 = 440; let pitch_set = generate_pitch_set(a4, 2, 2); write_pitches_to_file(pitch_set); -} \ No newline at end of file +} diff --git a/lib-stm32/src/anim/mod.rs b/lib-stm32/src/anim/mod.rs index c53cc2f2..c20c3f00 100644 --- a/lib-stm32/src/anim/mod.rs +++ b/lib-stm32/src/anim/mod.rs @@ -39,60 +39,46 @@ pub trait AnimInterface: Sized { ///////////////////////////////// pub enum Animation -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { Blink(BlinkAnimation), Lerp(LerpAnimation), } impl AnimInterface for Animation -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { fn get_id(&self) -> usize { match self { - Animation::Blink(a) => { - a.get_id() - }, - Animation::Lerp(a) => { - a.get_id() - }, + Animation::Blink(a) => a.get_id(), + Animation::Lerp(a) => a.get_id(), } } fn enable(&mut self) { match self { - Animation::Blink(a) => { - a.enable() - }, - Animation::Lerp(a) => { - a.enable() - }, + Animation::Blink(a) => a.enable(), + Animation::Lerp(a) => a.enable(), } } fn disable(&mut self) { match self { - Animation::Blink(a) => { - a.disable() - }, - Animation::Lerp(a) => { - a.disable() - }, + Animation::Blink(a) => a.disable(), + Animation::Lerp(a) => a.disable(), } } - + fn enabled(&self) -> bool { match self { - Animation::Blink(a) => { - a.enabled() - }, - Animation::Lerp(a) => { - a.enabled() - }, + Animation::Blink(a) => a.enabled(), + Animation::Lerp(a) => a.enabled(), } } @@ -100,10 +86,10 @@ L: crate::math::lerp::Lerp match self { Animation::Blink(a) => { a.start_animation(); - }, + } Animation::Lerp(a) => { a.start_animation(); - }, + } } } @@ -111,32 +97,24 @@ L: crate::math::lerp::Lerp match self { Animation::Blink(a) => { a.reset_animation(); - }, + } Animation::Lerp(a) => { a.reset_animation(); - }, + } } } fn animation_running(&self) -> bool { match self { - Animation::Blink(a) => { - a.animation_running() - }, - Animation::Lerp(a) => { - a.animation_running() - }, + Animation::Blink(a) => a.animation_running(), + Animation::Lerp(a) => a.animation_running(), } } fn animation_completed(&self) -> bool { match self { - Animation::Blink(a) => { - a.animation_completed() - }, - Animation::Lerp(a) => { - a.animation_completed() - }, + Animation::Blink(a) => a.animation_completed(), + Animation::Lerp(a) => a.animation_completed(), } } @@ -144,29 +122,27 @@ L: crate::math::lerp::Lerp match self { Animation::Blink(a) => { a.update(); - }, + } Animation::Lerp(a) => { a.update(); - }, + } } } fn get_value(&self) -> L { match self { - Animation::Blink(a) => { - a.get_value() - }, - Animation::Lerp(a) => { - a.get_value() - }, + Animation::Blink(a) => a.get_value(), + Animation::Lerp(a) => a.get_value(), } } } -pub struct CompositeAnimation<'a, N, L> -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp { +pub struct CompositeAnimation<'a, N, L> +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, +{ id: usize, animations: &'a mut [Animation], @@ -179,12 +155,17 @@ L: crate::math::lerp::Lerp { last_value: L, } -impl<'a, N, L> CompositeAnimation<'a, N, L> -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +impl<'a, N, L> CompositeAnimation<'a, N, L> +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { - pub fn new(id: usize, animations: &'a mut [Animation], repeat_mode: AnimRepeatMode) -> Self { + pub fn new( + id: usize, + animations: &'a mut [Animation], + repeat_mode: AnimRepeatMode, + ) -> Self { let first_val = animations[0].get_value(); CompositeAnimation { id, @@ -198,10 +179,11 @@ L: crate::math::lerp::Lerp } } -impl AnimInterface for CompositeAnimation<'_, N, L> -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +impl AnimInterface for CompositeAnimation<'_, N, L> +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { fn get_id(&self) -> usize { self.id @@ -270,17 +252,17 @@ L: crate::math::lerp::Lerp match self.repeat_mode { AnimRepeatMode::None => { self.anim_state = AnimState::Completed; - }, + } AnimRepeatMode::Fixed(_) => { if self.repeat_counter == 0 { self.anim_state = AnimState::Completed; } else { self.repeat_counter -= 1; } - }, + } AnimRepeatMode::Forever => { // we've already reset the animation index so it'll loop around - }, + } } } @@ -316,8 +298,15 @@ pub struct BlinkAnimation { } impl BlinkAnimation { - pub fn new(id: usize, val_one: T, val_two: T, v1_time: Duration, v2_time: Duration, repeat_style: AnimRepeatMode) -> Self { - BlinkAnimation { + pub fn new( + id: usize, + val_one: T, + val_two: T, + v1_time: Duration, + v2_time: Duration, + repeat_style: AnimRepeatMode, + ) -> Self { + BlinkAnimation { id, val_one, @@ -396,11 +385,11 @@ impl AnimInterface for BlinkAnimation { match self.repeat_style { AnimRepeatMode::None => { self.anim_state = AnimState::Completed; - }, + } AnimRepeatMode::Forever => { self.start_time = now; self.last_value = self.val_one; - }, + } AnimRepeatMode::Fixed(_) => { if self.repeat_counter == 0 { self.anim_state = AnimState::Completed; @@ -409,21 +398,23 @@ impl AnimInterface for BlinkAnimation { self.start_time = now; self.last_value = self.val_one; } - }, + } } } } - + fn get_value(&self) -> T { self.last_value } } #[derive(Clone, Copy, Debug)] -pub struct LerpAnimation -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp { +pub struct LerpAnimation +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, +{ id: usize, val_one: L, @@ -439,13 +430,20 @@ L: crate::math::lerp::Lerp { pd: PhantomData, } -impl LerpAnimation -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +impl LerpAnimation +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { - pub fn new(id: usize, val_one: L, val_two: L, lerp_duration_ms: Duration, repeat_style: AnimRepeatMode) -> Self { - LerpAnimation { + pub fn new( + id: usize, + val_one: L, + val_two: L, + lerp_duration_ms: Duration, + repeat_style: AnimRepeatMode, + ) -> Self { + LerpAnimation { id, val_one, @@ -463,10 +461,11 @@ L: crate::math::lerp::Lerp } } -impl AnimInterface for LerpAnimation -where N: LerpNumeric, -f32: core::convert::From, -L: crate::math::lerp::Lerp +impl AnimInterface for LerpAnimation +where + N: LerpNumeric, + f32: core::convert::From, + L: crate::math::lerp::Lerp, { fn get_id(&self) -> usize { self.id @@ -528,11 +527,11 @@ L: crate::math::lerp::Lerp match self.repeat_style { AnimRepeatMode::None => { self.anim_state = AnimState::Completed; - }, + } AnimRepeatMode::Forever => { self.start_time = now; self.last_value = self.val_one; - }, + } AnimRepeatMode::Fixed(_) => { if self.repeat_counter == 0 { self.anim_state = AnimState::Completed; @@ -541,11 +540,11 @@ L: crate::math::lerp::Lerp self.start_time = now; self.last_value = self.val_one; } - }, + } } } } - + fn get_value(&self) -> L { self.last_value } diff --git a/lib-stm32/src/audio/mod.rs b/lib-stm32/src/audio/mod.rs index 19efd3b8..b376f322 100644 --- a/lib-stm32/src/audio/mod.rs +++ b/lib-stm32/src/audio/mod.rs @@ -1,17 +1,17 @@ use songs::SongId; pub mod note; +pub mod pitches; pub mod songs; pub mod tone_player; -pub mod pitches; #[derive(Debug)] pub enum AudioError { - UnplayablePitch + UnplayablePitch, } #[derive(Clone, Copy, Debug)] pub enum AudioCommand { PlaySong(SongId), Stop, -} \ No newline at end of file +} diff --git a/lib-stm32/src/audio/note.rs b/lib-stm32/src/audio/note.rs index c5d5bdb5..38d820a0 100644 --- a/lib-stm32/src/audio/note.rs +++ b/lib-stm32/src/audio/note.rs @@ -1,6 +1,6 @@ pub enum Beat { - Note {tone: u16, duration: u32}, + Note { tone: u16, duration: u32 }, Rest(u32), } -pub type Song = [Beat]; \ No newline at end of file +pub type Song = [Beat]; diff --git a/lib-stm32/src/audio/songs.rs b/lib-stm32/src/audio/songs.rs index a28ad3b0..10a4bb8a 100644 --- a/lib-stm32/src/audio/songs.rs +++ b/lib-stm32/src/audio/songs.rs @@ -8,5 +8,5 @@ pub enum SongId { BatteryCritical, BatteryUhOh, BalanceConnected, - BalanceDisconnected -} \ No newline at end of file + BalanceDisconnected, +} diff --git a/lib-stm32/src/audio/tone_player.rs b/lib-stm32/src/audio/tone_player.rs index 44d9d318..2abdc6a8 100644 --- a/lib-stm32/src/audio/tone_player.rs +++ b/lib-stm32/src/audio/tone_player.rs @@ -1,9 +1,10 @@ - use embassy_time::Timer; +use super::{ + note::{Beat, Song}, + AudioError, +}; use crate::drivers::audio::PlayTone; -use super::{note::{Beat, Song}, AudioError}; - pub struct TonePlayer<'a, D: PlayTone> { audio_driver: D, @@ -21,16 +22,15 @@ impl<'a, D: PlayTone> TonePlayer<'a, D> { pub fn load_song(&mut self, song: &'a Song) -> Result<(), AudioError> { let mut can_play = true; - 'note_loop: - for beat in song.iter() { + 'note_loop: for beat in song.iter() { match beat { Beat::Note { tone, duration: _ } => { if !self.audio_driver.can_play_tone(*tone) { can_play = false; break 'note_loop; } - }, - Beat::Rest(_) => { }, + } + Beat::Rest(_) => {} } } @@ -50,15 +50,15 @@ impl<'a, D: PlayTone> TonePlayer<'a, D> { Beat::Note { tone, duration } => { self.audio_driver.play_tone(*tone); Timer::after_micros(*duration as u64).await; - }, + } Beat::Rest(duration) => { self.audio_driver.play_tone(0); Timer::after_micros(*duration as u64).await; - }, + } } } self.audio_driver.play_tone(0); } } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/adc.rs b/lib-stm32/src/drivers/adc.rs index 807c2cb5..e3822983 100644 --- a/lib-stm32/src/drivers/adc.rs +++ b/lib-stm32/src/drivers/adc.rs @@ -9,12 +9,17 @@ pub struct AdcConverter { } impl AdcConverter { - pub const fn new(vref_ext_mv: u16, vref_int_mv: u16, use_vref_int: bool, use_temp: bool) -> Self { + pub const fn new( + vref_ext_mv: u16, + vref_int_mv: u16, + use_vref_int: bool, + use_temp: bool, + ) -> Self { AdcConverter { use_vref_int, use_temp, - vref_ext_mv, - vref_int_mv, + vref_ext_mv, + vref_int_mv, last_vrefint: 0, } } @@ -27,7 +32,7 @@ impl AdcConverter { ((sample as u32 / 4096) * (self.vref_ext_mv as u32) * (MV_PER_V as u32)) as u16 } - fn raw_sample_to_mv_vrefint_comp(&self, sample: u16) -> u16 { + fn raw_sample_to_mv_vrefint_comp(&self, sample: u16) -> u16 { (u32::from(sample) * u32::from(self.vref_int_mv) / u32::from(self.last_vrefint)) as u16 } @@ -66,4 +71,4 @@ impl AdcConverter { *mv_sample = self.raw_sample_to_v(*raw_sample); } } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/audio/buzzer.rs b/lib-stm32/src/drivers/audio/buzzer.rs index a6ef385c..ff655384 100644 --- a/lib-stm32/src/drivers/audio/buzzer.rs +++ b/lib-stm32/src/drivers/audio/buzzer.rs @@ -1,4 +1,10 @@ -use embassy_stm32::{time::hz, timer::{simple_pwm::{SimplePwm, SimplePwmChannel}, Channel, GeneralInstance4Channel}}; +use embassy_stm32::{ + time::hz, + timer::{ + simple_pwm::{SimplePwm, SimplePwmChannel}, + Channel, GeneralInstance4Channel, + }, +}; use num_traits::clamp; use super::PlayTone; @@ -19,7 +25,12 @@ impl<'d, T: GeneralInstance4Channel> Buzzer<'d, T> { Self::new_with_fcontrs(pwm, channel, BUZZER_MIN_FREQ, BUZZER_MAX_FREQ) } - pub fn new_with_fcontrs(pwm: SimplePwm<'d, T>, channel: Channel, min_freq: u16, max_freq: u16) -> Self { + pub fn new_with_fcontrs( + pwm: SimplePwm<'d, T>, + channel: Channel, + min_freq: u16, + max_freq: u16, + ) -> Self { Self { pwm, channel: channel, @@ -60,4 +71,4 @@ impl PlayTone for Buzzer<'_, T> { fn can_play_tone(&self, freq: u16) -> bool { self.min_freq < freq && freq < self.max_freq } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/audio/mod.rs b/lib-stm32/src/drivers/audio/mod.rs index 9b58d074..65547de6 100644 --- a/lib-stm32/src/drivers/audio/mod.rs +++ b/lib-stm32/src/drivers/audio/mod.rs @@ -3,4 +3,4 @@ pub mod buzzer; pub trait PlayTone { fn play_tone(&mut self, tone: u16); fn can_play_tone(&self, tone: u16) -> bool; -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/boot/mod.rs b/lib-stm32/src/drivers/boot/mod.rs index bada1c33..2dc66ea5 100644 --- a/lib-stm32/src/drivers/boot/mod.rs +++ b/lib-stm32/src/drivers/boot/mod.rs @@ -1 +1 @@ -pub mod stm32_interface; \ No newline at end of file +pub mod stm32_interface; diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index 06c79ecd..2cb0c6d2 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -2,10 +2,10 @@ use core::cmp::min; use defmt_rtt as _; -use embassy_stm32::gpio::{Level, Output, Pin, Speed, Pull}; +use embassy_stm32::gpio::{Level, Output, Pin, Pull, Speed}; use embassy_stm32::usart::{self, Config, DataBits, Parity, StopBits}; -use embassy_time::{Duration, Timer}; use embassy_time::with_timeout; +use embassy_time::{Duration, Timer}; use crate::queue::{DequeueRef, Error}; use crate::uart::queue::{IdleBufferedUart, Reader, UartReadQueue, UartWriteQueue, Writer}; @@ -38,12 +38,12 @@ pub fn get_bootloader_uart_config() -> Config { } pub struct Stm32Interface< - 'a, - const LEN_RX: usize, - const LEN_TX: usize, - const DEPTH_RX: usize, - const DEPTH_TX: usize, - const DEBUG_UART_QUEUES: bool, + 'a, + const LEN_RX: usize, + const LEN_TX: usize, + const DEPTH_RX: usize, + const DEPTH_TX: usize, + const DEBUG_UART_QUEUES: bool, > { uart: &'a IdleBufferedUart, reader: &'a UartReadQueue, @@ -146,10 +146,10 @@ impl< // ensure UART is in the expected config for the bootloader // this operation is unsafe because it takes the uart module offline // when the executor may be relying on rx interrupts to unblock a thread - self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven).await; + self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven) + .await; Timer::after_millis(100).await; - // set the boot0 line high to enter the UART bootloader upon reset self.boot0_pin.set_high(); Timer::after_millis(1).await; @@ -161,37 +161,42 @@ impl< // this time isn't documented and can possibly be lowered. Timer::after(Duration::from_millis(10)).await; - - defmt::debug!("sending the bootloader baud calibration command..."); Timer::after_millis(1000).await; - if self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CODE_SEQUENCE_BYTE; - 1 - }) - .await.is_err() { + if self + .writer + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CODE_SEQUENCE_BYTE; + 1 + }) + .await + .is_err() + { defmt::debug!("failed to send bootloader start seq"); return Err(()); } Timer::after_millis(10).await; let mut res = Err(()); - let sync_res = with_timeout(Duration::from_millis(5000), self.reader.read(|buf| { - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - defmt::debug!("bootloader replied with ACK after calibration."); - self.in_bootloader = true; - res = Ok(()); - } else { - defmt::debug!("bootloader replied with NACK after calibration."); + let sync_res = with_timeout( + Duration::from_millis(5000), + self.reader.read(|buf| { + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + defmt::debug!("bootloader replied with ACK after calibration."); + self.in_bootloader = true; + res = Ok(()); + } else { + defmt::debug!("bootloader replied with NACK after calibration."); + } } - } - })).await; + }), + ) + .await; if sync_res.is_err() { defmt::warn!("*** HARDWARE CHECK *** - bootloader baud calibration timed out."); - return Err(()) + return Err(()); } res @@ -266,30 +271,38 @@ impl< } self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_GET; - buf[1] = !STM32_BOOTLOADER_CMD_GET; - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_GET; + buf[1] = !STM32_BOOTLOADER_CMD_GET; + 2 + }) + .await?; let mut res = Err(()); - self.reader.read(|buf| { - // TODO discovery and return checksum capability and erase/ext erase capability - if buf.len() == 1 && buf[0] == STM32_BOOTLOADER_NACK { - defmt::error!("failed to read bootloader command list. (NACK)."); - res = Err(()); - } else if buf.len() == 15 && buf[0] == STM32_BOOTLOADER_ACK && buf[14] == STM32_BOOTLOADER_ACK { - defmt::debug!("read bootloader command list. (NO AUTO CS)."); - res = Ok(()); - } else if buf.len() == 16 && buf[0] == STM32_BOOTLOADER_ACK && buf[15] == STM32_BOOTLOADER_ACK { - defmt::debug!("read bootloader command list. (WITH CS)."); - res = Ok(()); - } else { - defmt::error!("unknown command enumeration error: {}", buf); - res = Err(()); - } - }).await?; + self.reader + .read(|buf| { + // TODO discovery and return checksum capability and erase/ext erase capability + if buf.len() == 1 && buf[0] == STM32_BOOTLOADER_NACK { + defmt::error!("failed to read bootloader command list. (NACK)."); + res = Err(()); + } else if buf.len() == 15 + && buf[0] == STM32_BOOTLOADER_ACK + && buf[14] == STM32_BOOTLOADER_ACK + { + defmt::debug!("read bootloader command list. (NO AUTO CS)."); + res = Ok(()); + } else if buf.len() == 16 + && buf[0] == STM32_BOOTLOADER_ACK + && buf[15] == STM32_BOOTLOADER_ACK + { + defmt::debug!("read bootloader command list. (WITH CS)."); + res = Ok(()); + } else { + defmt::error!("unknown command enumeration error: {}", buf); + res = Err(()); + } + }) + .await?; res } @@ -305,31 +318,33 @@ impl< } self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_GET_ID; - buf[1] = !STM32_BOOTLOADER_CMD_GET_ID; - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_GET_ID; + buf[1] = !STM32_BOOTLOADER_CMD_GET_ID; + 2 + }) + .await?; let mut res = Err(()); - self.reader.read(|buf| { - if buf.len() == 1 && buf[0] == STM32_BOOTLOADER_NACK { - defmt::error!("could not read device ID."); - res = Err(()); - } else if buf.len() == 5 && buf[1] == 1 && buf[4] == STM32_BOOTLOADER_ACK { - let pid: u16 = buf[3] as u16; - defmt::trace!("found 1 byte pid {:?}", pid); - res = Ok(pid); - } else if buf.len() == 5 && buf[1] == 2 && buf[4] == STM32_BOOTLOADER_ACK { - let pid: u16 = ((buf[2] as u16) << 8) | (buf[3] as u16); - defmt::trace!("found 2 byte pid {:?}", pid); - res = Ok(pid); - } else { - defmt::error!("malformed response in device ID read."); - res = Err(()); - } - }).await?; + self.reader + .read(|buf| { + if buf.len() == 1 && buf[0] == STM32_BOOTLOADER_NACK { + defmt::error!("could not read device ID."); + res = Err(()); + } else if buf.len() == 5 && buf[1] == 1 && buf[4] == STM32_BOOTLOADER_ACK { + let pid: u16 = buf[3] as u16; + defmt::trace!("found 1 byte pid {:?}", pid); + res = Ok(pid); + } else if buf.len() == 5 && buf[1] == 2 && buf[4] == STM32_BOOTLOADER_ACK { + let pid: u16 = ((buf[2] as u16) << 8) | (buf[3] as u16); + defmt::trace!("found 2 byte pid {:?}", pid); + res = Ok(pid); + } else { + defmt::error!("malformed response in device ID read."); + res = Err(()); + } + }) + .await?; res } @@ -352,24 +367,26 @@ impl< // defmt::debug!("sending the write command..."); self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_WRITE_MEM; - buf[1] = !STM32_BOOTLOADER_CMD_WRITE_MEM; - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_WRITE_MEM; + buf[1] = !STM32_BOOTLOADER_CMD_WRITE_MEM; + 2 + }) + .await?; let mut res = Err(()); - 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!("write mem replied with NACK"); + 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!("write mem replied with NACK"); + } } - } - }).await?; + }) + .await?; if res.is_err() { return res; @@ -377,63 +394,71 @@ impl< // defmt::debug!("sending the load address {:X}...", write_base_addr); self.writer - .write(|buf| { - let sa_bytes: [u8; 4] = write_base_addr.to_be_bytes(); - let cs = Self::bootloader_checksum_u32(write_base_addr); - buf[0] = sa_bytes[0]; - buf[1] = sa_bytes[1]; - buf[2] = sa_bytes[2]; - buf[3] = sa_bytes[3]; - buf[4] = cs; - // defmt::trace!("send load address buffer {:X}", buf); - 5 - }) - .await?; + .write(|buf| { + let sa_bytes: [u8; 4] = write_base_addr.to_be_bytes(); + let cs = Self::bootloader_checksum_u32(write_base_addr); + buf[0] = sa_bytes[0]; + buf[1] = sa_bytes[1]; + buf[2] = sa_bytes[2]; + buf[3] = sa_bytes[3]; + buf[4] = cs; + // defmt::trace!("send load address buffer {:X}", buf); + 5 + }) + .await?; // defmt::debug!("wait for load address reply"); - self.reader.read(|buf| { - defmt::trace!("load address reply {:X}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); - // defmt::trace!("load address accepted."); - } else { - defmt::error!("load address rejected (NACK)"); + self.reader + .read(|buf| { + defmt::trace!("load address reply {:X}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + // defmt::trace!("load address accepted."); + } else { + defmt::error!("load address rejected (NACK)"); + } } - } - }).await?; + }) + .await?; // defmt::debug!("sending the data..."); self.writer - .write(|buf| { - let cs = Self::bootloader_checksum_buf(data); - let data_len = data.len(); - // defmt::trace!("firmware data buffer len {:?}", data_len); - buf[0] = data_len as u8 - 1; - buf[1..(data_len + 1)].copy_from_slice(data); - buf[data_len + 1] = cs; - // defmt::trace!("send data buffer {:X}", buf); - data.len() + 2 - }) - .await?; + .write(|buf| { + let cs = Self::bootloader_checksum_buf(data); + let data_len = data.len(); + // defmt::trace!("firmware data buffer len {:?}", data_len); + buf[0] = data_len as u8 - 1; + buf[1..(data_len + 1)].copy_from_slice(data); + buf[data_len + 1] = cs; + // defmt::trace!("send data buffer {:X}", buf); + data.len() + 2 + }) + .await?; // defmt::debug!("wait send data reply"); - self.reader.read(|buf| { - // defmt::trace!("send data reply {:X}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); - // defmt::trace!("data accepted."); - } else { - defmt::error!("data rejected (NACK)"); + self.reader + .read(|buf| { + // defmt::trace!("send data reply {:X}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + // defmt::trace!("data accepted."); + } else { + defmt::error!("data rejected (NACK)"); + } } - } - }).await?; + }) + .await?; Ok(()) } - pub async fn write_device_memory(&self, buf: &[u8], write_base_addr: Option) -> Result<(), ()> { + pub async fn write_device_memory( + &self, + buf: &[u8], + write_base_addr: Option, + ) -> Result<(), ()> { if !self.in_bootloader { defmt::error!("called bootloader operation when not in bootloader context."); return Err(()); @@ -446,7 +471,7 @@ impl< if step_size < 8 { defmt::error!("bootloader buffer too small."); return Err(()); - } + } // if user doesn't supply a start address, assume base of mapped flash let mut addr = write_base_addr.unwrap_or(0x0800_0000); @@ -454,7 +479,8 @@ impl< for start in (0..buf.len()).step_by(step_size) { let end = core::cmp::min(start + step_size, buf.len()); - self.write_device_memory_chunk( &buf[start..end], addr).await?; + self.write_device_memory_chunk(&buf[start..end], addr) + .await?; addr += step_size as u32; } @@ -471,52 +497,60 @@ impl< 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?; + .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::trace!("extended erase reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); - } else { - defmt::error!("bootloader replied with NACK"); + self.reader + .read(|buf| { + defmt::trace!("extended erase reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("bootloader replied with NACK"); + } } - } - }).await?; + }) + .await?; defmt::debug!("sending erase type..."); self.writer - .write(|buf| { - buf[0] = 0xFF; - buf[1] = 0xFF; - buf[2] = 0x00; - 3 - }) - .await?; + .write(|buf| { + buf[0] = 0xFF; + buf[1] = 0xFF; + buf[2] = 0x00; + 3 + }) + .await?; let mut res = Err(()); - self.reader.read(|buf| { - defmt::debug!("erase reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - defmt::debug!("flash erased"); - res = Ok(()); - } else { - defmt::error!("bootloader replied with NACK"); + self.reader + .read(|buf| { + defmt::debug!("erase reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + defmt::debug!("flash erased"); + res = Ok(()); + } else { + defmt::error!("bootloader replied with NACK"); + } } - } - }).await?; + }) + .await?; Ok(()) } - pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8], leave_in_reset: bool) -> Result<(), ()> { + pub async fn load_firmware_image( + &mut self, + fw_image_bytes: &[u8], + leave_in_reset: bool, + ) -> Result<(), ()> { if self.in_bootloader { defmt::trace!("device is already in bootloader"); } else { @@ -546,7 +580,7 @@ impl< defmt::error!("found unknown device id {}", device_id); return Err(()); } - } + }, }; // erase part @@ -577,31 +611,32 @@ impl< let start_address = start_address.unwrap_or(0x0800_0000); // let start_address = start_address.unwrap_or(0x20002000); - // set the boot0 line low to disable startup bootloader // not needed for command, but makes sense on principle self.boot0_pin.set_low(); defmt::debug!("sending the go command..."); self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_GO; - buf[1] = !STM32_BOOTLOADER_CMD_GO; - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_GO; + buf[1] = !STM32_BOOTLOADER_CMD_GO; + 2 + }) + .await?; let mut res = Err(()); - 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!("bootloader replied with NACK"); + 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!("bootloader replied with NACK"); + } } - } - }).await?; + }) + .await?; if res.is_err() { return res; @@ -609,33 +644,35 @@ impl< defmt::debug!("sending the start address {:?}...", start_address); self.writer - .write(|buf| { - let sa_bytes: [u8; 4] = start_address.to_be_bytes(); - let cs = Self::bootloader_checksum_u32(start_address); - buf[0] = sa_bytes[0]; - buf[1] = sa_bytes[1]; - buf[2] = sa_bytes[2]; - buf[3] = sa_bytes[3]; - buf[4] = cs; - // defmt::debug!("send buffer {:?}", buf); - 5 - }) - .await?; + .write(|buf| { + let sa_bytes: [u8; 4] = start_address.to_be_bytes(); + let cs = Self::bootloader_checksum_u32(start_address); + buf[0] = sa_bytes[0]; + buf[1] = sa_bytes[1]; + buf[2] = sa_bytes[2]; + buf[3] = sa_bytes[3]; + buf[4] = cs; + // defmt::debug!("send buffer {:?}", buf); + 5 + }) + .await?; defmt::debug!("wait for sa reply"); - self.reader.read(|buf| { - defmt::info!("sa reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); - self.in_bootloader = false; - defmt::info!("program started."); - } else { - defmt::error!("bootloader replied with NACK"); + self.reader + .read(|buf| { + defmt::info!("sa reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + self.in_bootloader = false; + defmt::info!("program started."); + } else { + defmt::error!("bootloader replied with NACK"); + } } - } - }).await?; + }) + .await?; res } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index 51bb0322..01bcd687 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -1,7 +1,7 @@ use embassy_stm32::{ - gpio::{Level, Output, Pin, Speed}, + gpio::{Level, Output, Pin, Speed}, mode::Async, - spi::{self, Error} + spi::{self, Error}, }; pub struct AT25DF041B<'buf, const CS_POL_N: bool> { @@ -12,10 +12,12 @@ pub struct AT25DF041B<'buf, const CS_POL_N: bool> { } impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { - pub fn new(spi: spi::Spi<'static, Async>, + pub fn new( + spi: spi::Spi<'static, Async>, chip_select: impl Pin, tx_buf: &'buf mut [u8; 256], - rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, CS_POL_N> { + rx_buf: &'buf mut [u8; 256], + ) -> AT25DF041B<'buf, CS_POL_N> { AT25DF041B { spi, chip_select: Output::new(chip_select, Level::High, Speed::High), @@ -43,8 +45,9 @@ impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { async fn transfer(&mut self, len: usize) -> Result<(), Error> { let len = if len > 256 { 256 } else { len }; - - self.spi.transfer(&mut self.rx_buf[0..len], self.tx_buf).await + self.spi + .transfer(&mut self.rx_buf[0..len], self.tx_buf) + .await } pub async fn verify_chip_id(&mut self) -> Result<(), ()> { @@ -70,9 +73,9 @@ impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { if received_mfg_info != expected_mfg_info { defmt::error!("AT25DF041B manufacturer information did not match datasheet."); defmt::debug!("expected {}, got {}", expected_mfg_info, received_mfg_info); - return Err(()) + return Err(()); } Ok(()) } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/flash/mod.rs b/lib-stm32/src/drivers/flash/mod.rs index 2b0d9cc0..0b833498 100644 --- a/lib-stm32/src/drivers/flash/mod.rs +++ b/lib-stm32/src/drivers/flash/mod.rs @@ -1,2 +1 @@ - -pub mod at25df041b; \ No newline at end of file +pub mod at25df041b; diff --git a/lib-stm32/src/drivers/imu/bmi085.rs b/lib-stm32/src/drivers/imu/bmi085.rs index 38090894..0e2c0596 100644 --- a/lib-stm32/src/drivers/imu/bmi085.rs +++ b/lib-stm32/src/drivers/imu/bmi085.rs @@ -1,8 +1,8 @@ /* * Driver for the Bosch BMI085 IMU. - * + * * https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi085-ds001.pdf - * + * */ use core::{cmp::min, f32::consts::PI}; @@ -12,13 +12,12 @@ use embassy_stm32::{ mode::Async, spi::{self, MisoPin, MosiPin, SckPin}, time::hz, - Peripheral + Peripheral, }; -use embassy_time::{Timer, Duration}; +use embassy_time::{Duration, Timer}; use defmt::*; - pub const SPI_MIN_BUF_LEN: usize = 8; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro @@ -200,12 +199,11 @@ const GYRO_CHIP_ID: u8 = 0x0F; const READ_BIT: u8 = 0x80; -impl<'a, 'buf> - Bmi085<'a, 'buf> { +impl<'a, 'buf> Bmi085<'a, 'buf> { /// creates a new BMI085 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, Async>, - accel_cs: Output<'a>, + spi: spi::Spi<'a, Async>, + accel_cs: Output<'a>, gyro_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { @@ -234,20 +232,12 @@ impl<'a, 'buf> let mut spi_config = spi::Config::default(); spi_config.frequency = hz(1_000_000); - let imu_spi = spi::Spi::new( - peri, - sck, - mosi, - miso, - txdma, - rxdma, - spi_config - ); + let imu_spi = spi::Spi::new(peri, sck, mosi, miso, txdma, rxdma, spi_config); let accel_cs = Output::new(accel_cs, Level::High, Speed::VeryHigh); let imu_cs = Output::new(gyro_cs, Level::High, Speed::VeryHigh); - Bmi085 { + Bmi085 { spi: imu_spi, accel_cs, gyro_cs: imu_cs, @@ -279,25 +269,31 @@ impl<'a, 'buf> } async fn accel_burst_read(&mut self, reg: AccelRegisters, dest: &mut [u8]) { - // the transaction length is either the dest buf size + 2 + // the transaction length is either the dest buf size + 2 // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) // OR upper bounded by internal length of the buffer. let trx_len = min(dest.len() + 2, self.spi_buf.len()); self.spi_buf[0] = reg as u8 | READ_BIT; self.spi_buf[1] = 0x00; - let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + let _ = self + .spi + .transfer_in_place(&mut self.spi_buf[..trx_len]) + .await; dest.copy_from_slice(&self.spi_buf[2..trx_len]); } async fn gyro_burst_read(&mut self, reg: GyroRegisters, dest: &mut [u8]) { - // the transaction length is either the dest buf size + 1 + // the transaction length is either the dest buf size + 1 // (the start addr + N data bytes) // OR upper bounded by internal length of the buffer. let trx_len = min(dest.len() + 1, self.spi_buf.len()); self.spi_buf[0] = reg as u8 | READ_BIT; - let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + let _ = self + .spi + .transfer_in_place(&mut self.spi_buf[..trx_len]) + .await; dest.copy_from_slice(&self.spi_buf[1..trx_len]); } @@ -340,18 +336,31 @@ impl<'a, 'buf> // procedure is described in section 4.6.1 of the datasheet self.accel_set_range(AccelRange::Range16g).await; - self.accel_set_bandwidth(AccelConfBwp::OverSamplingNormal, AccelConfOdr::OutputDataRate1600p0).await; + self.accel_set_bandwidth( + AccelConfBwp::OverSamplingNormal, + AccelConfOdr::OutputDataRate1600p0, + ) + .await; Timer::after(Duration::from_millis(2 + 1)).await; - self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::PositiveSelfTest as u8).await; + self.accel_write( + AccelRegisters::ACC_SELF_TEST, + AccelSelfTestReg::PositiveSelfTest as u8, + ) + .await; Timer::after(Duration::from_millis(50 + 1)).await; let positive_test_data = self.accel_get_data_mg().await; - self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::NegativeSelfTest as u8).await; + self.accel_write( + AccelRegisters::ACC_SELF_TEST, + AccelSelfTestReg::NegativeSelfTest as u8, + ) + .await; Timer::after(Duration::from_millis(50 + 1)).await; let negative_test_data = self.accel_get_data_mg().await; - self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::Off as u8).await; + self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::Off as u8) + .await; Timer::after(Duration::from_millis(50 + 1)).await; let x_diff = positive_test_data[0] - negative_test_data[0]; @@ -360,17 +369,26 @@ impl<'a, 'buf> let mut result = Ok(()); if x_diff < 1000.0 { - trace!("accel self test failed x-axis needed polarity difference >=1000 got {}", x_diff); + trace!( + "accel self test failed x-axis needed polarity difference >=1000 got {}", + x_diff + ); result = Err(()) } if y_diff < 1000.0 { - trace!("accel self test failed y-axis needed polarity difference >=1000 got {}", y_diff); + trace!( + "accel self test failed y-axis needed polarity difference >=1000 got {}", + y_diff + ); result = Err(()) } if z_diff < 500.0 { - trace!("accel self test failed z-axis needed polarity difference >=500 got {}", z_diff); + trace!( + "accel self test failed z-axis needed polarity difference >=500 got {}", + z_diff + ); result = Err(()) } @@ -382,7 +400,11 @@ impl<'a, 'buf> } async fn gyro_self_test(&mut self) -> Result<(), ()> { - self.gyro_write(GyroRegisters::GYRO_SELF_TEST, GyroSelfTestReg::TrigBist as u8).await; + self.gyro_write( + GyroRegisters::GYRO_SELF_TEST, + GyroSelfTestReg::TrigBist as u8, + ) + .await; let mut try_ct = 0; loop { let strreg_val = self.gyro_read(GyroRegisters::GYRO_SELF_TEST).await; @@ -414,7 +436,10 @@ impl<'a, 'buf> let _ = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; let acc_chip_id = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; if acc_chip_id != ACCEL_CHIP_ID { - warn!("read accel ID (0x{:x}) does not match expected BMI085 accel ID (0x{:x})", acc_chip_id, ACCEL_CHIP_ID); + warn!( + "read accel ID (0x{:x}) does not match expected BMI085 accel ID (0x{:x})", + acc_chip_id, ACCEL_CHIP_ID + ); has_self_test_error = Err(()); } else { debug!("accel id verified: 0x{:x}", acc_chip_id); @@ -423,12 +448,15 @@ impl<'a, 'buf> let _ = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; let gyro_chip_id = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; if gyro_chip_id != GYRO_CHIP_ID { - warn!("read gyro ID (0x{:x}) does not match expected BMI085 gyro ID (0x{:x})", gyro_chip_id, GYRO_CHIP_ID); + warn!( + "read gyro ID (0x{:x}) does not match expected BMI085 gyro ID (0x{:x})", + gyro_chip_id, GYRO_CHIP_ID + ); has_self_test_error = Err(()); } else { debug!("gyro id verified: 0x{:x}", gyro_chip_id); } - + if (self.accel_self_test().await).is_err() { has_self_test_error = Err(()); } @@ -443,14 +471,17 @@ impl<'a, 'buf> const fn read_pair_to_i16(&self, lsb: u8, msb: u8) -> i16 { (msb as u16 * 256 + lsb as u16) as i16 } - + pub async fn accel_get_raw_data(&mut self) -> [i16; 3] { let mut buf: [u8; 6] = [0; 6]; - self.accel_burst_read(AccelRegisters::ACC_X_LSB, &mut buf).await; + self.accel_burst_read(AccelRegisters::ACC_X_LSB, &mut buf) + .await; - [self.read_pair_to_i16(buf[0], buf[1]), + [ + self.read_pair_to_i16(buf[0], buf[1]), self.read_pair_to_i16(buf[2], buf[3]), - self.read_pair_to_i16(buf[4], buf[5])] + self.read_pair_to_i16(buf[4], buf[5]), + ] } pub fn accel_range_mg(&self) -> f32 { @@ -475,36 +506,52 @@ impl<'a, 'buf> pub async fn accel_get_data_mg(&mut self) -> [f32; 3] { let raw_data = self.accel_get_raw_data().await; - [self.convert_accel_raw_sample_mg(raw_data[0]), + [ + self.convert_accel_raw_sample_mg(raw_data[0]), self.convert_accel_raw_sample_mg(raw_data[1]), - self.convert_accel_raw_sample_mg(raw_data[2])] + self.convert_accel_raw_sample_mg(raw_data[2]), + ] } pub async fn accel_get_data_mps(&mut self) -> [f32; 3] { let raw_data = self.accel_get_raw_data().await; - [self.convert_accel_raw_sample_mps(raw_data[0]), + [ + self.convert_accel_raw_sample_mps(raw_data[0]), self.convert_accel_raw_sample_mps(raw_data[1]), - self.convert_accel_raw_sample_mps(raw_data[2])] + self.convert_accel_raw_sample_mps(raw_data[2]), + ] } - pub async fn accel_set_bandwidth(&mut self, oversampling_mode: AccelConfBwp, output_data_rate: AccelConfOdr) { - self.accel_write(AccelRegisters::ACC_CONF, (oversampling_mode as u8) << 4 | output_data_rate as u8 ).await; + pub async fn accel_set_bandwidth( + &mut self, + oversampling_mode: AccelConfBwp, + output_data_rate: AccelConfOdr, + ) { + self.accel_write( + AccelRegisters::ACC_CONF, + (oversampling_mode as u8) << 4 | output_data_rate as u8, + ) + .await; } pub async fn accel_set_range(&mut self, range: AccelRange) { - self.accel_write(AccelRegisters::ACC_RANGE, range as u8).await; + self.accel_write(AccelRegisters::ACC_RANGE, range as u8) + .await; self.accel_range = range; } pub async fn gyro_get_raw_data(&mut self) -> [i16; 3] { let mut buf: [u8; 6] = [0; 6]; - self.gyro_burst_read(GyroRegisters::RATE_X_LSB, &mut buf).await; + self.gyro_burst_read(GyroRegisters::RATE_X_LSB, &mut buf) + .await; - [self.read_pair_to_i16(buf[0], buf[1]), + [ + self.read_pair_to_i16(buf[0], buf[1]), self.read_pair_to_i16(buf[2], buf[3]), - self.read_pair_to_i16(buf[4], buf[5])] + self.read_pair_to_i16(buf[4], buf[5]), + ] } pub fn dps_to_rads(dps: f32) -> f32 { @@ -542,47 +589,68 @@ impl<'a, 'buf> pub async fn gyro_get_data_dps(&mut self) -> [f32; 3] { let raw_data = self.gyro_get_raw_data().await; - [self.convert_raw_gyro_sample_dps(raw_data[0]), + [ + self.convert_raw_gyro_sample_dps(raw_data[0]), self.convert_raw_gyro_sample_dps(raw_data[1]), - self.convert_raw_gyro_sample_dps(raw_data[2])] + self.convert_raw_gyro_sample_dps(raw_data[2]), + ] } pub async fn gyro_get_data_rads(&mut self) -> [f32; 3] { let raw_data = self.gyro_get_raw_data().await; - [self.convert_raw_gyro_sample_rads(raw_data[0]), + [ + self.convert_raw_gyro_sample_rads(raw_data[0]), self.convert_raw_gyro_sample_rads(raw_data[1]), - self.convert_raw_gyro_sample_rads(raw_data[2])] + self.convert_raw_gyro_sample_rads(raw_data[2]), + ] } pub async fn gyro_set_range(&mut self, range: GyroRange) { - self.gyro_write(GyroRegisters::GYRO_RANGE, range as u8).await; + self.gyro_write(GyroRegisters::GYRO_RANGE, range as u8) + .await; self.gyro_range = range; } pub async fn gyro_set_bandwidth(&mut self, bandwidth: GyroBandwidth) { - self.gyro_write(GyroRegisters::GYRO_BANDWIDTH, bandwidth as u8).await; + self.gyro_write(GyroRegisters::GYRO_BANDWIDTH, bandwidth as u8) + .await; } pub async fn gyro_enable_interrupts(&mut self) { - self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOnNewData as u8).await; + self.gyro_write( + GyroRegisters::GYRO_INT_CONTROL, + GyroIntCtrl::InterruptOnNewData as u8, + ) + .await; } pub async fn gyro_disable_interrupts(&mut self) { - self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOff as u8).await; + self.gyro_write( + GyroRegisters::GYRO_INT_CONTROL, + GyroIntCtrl::InterruptOff as u8, + ) + .await; } - pub async fn gyro_set_int_config(&mut self, + pub async fn gyro_set_int_config( + &mut self, int3_active_state: GyroIntPinActiveState, int3_mode: GyroIntPinMode, int4_active_state: GyroIntPinActiveState, - int4_mode: GyroIntPinMode) { - let reg_val = (int4_mode as u8) << 3 | (int4_active_state as u8) << 2 | (int3_mode as u8) << 1 | int3_active_state as u8; - self.gyro_write(GyroRegisters::INT3_INT4_IO_CONF, reg_val).await; + int4_mode: GyroIntPinMode, + ) { + let reg_val = (int4_mode as u8) << 3 + | (int4_active_state as u8) << 2 + | (int3_mode as u8) << 1 + | int3_active_state as u8; + self.gyro_write(GyroRegisters::INT3_INT4_IO_CONF, reg_val) + .await; } pub async fn gyro_set_int_map(&mut self, map: GyroIntMap) { - self.gyro_write(GyroRegisters::INT3_INT4_IO_MAP, map as u8).await; + self.gyro_write(GyroRegisters::INT3_INT4_IO_MAP, map as u8) + .await; } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 0d24b4fe..8f066029 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -1,7 +1,7 @@ /* * Driver for the Bosch BMI323 IMU. - * - * https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi323-ds000.pdf * + * + * https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi323-ds000.pdf * */ use core::{cmp::min, f32::consts::PI}; @@ -11,7 +11,7 @@ use embassy_stm32::{ mode::Async, spi::{self, MisoPin, MosiPin, SckPin}, time::hz, - Peripheral + Peripheral, }; pub const SPI_MIN_BUF_LEN: usize = 14; @@ -170,7 +170,6 @@ pub enum IntPinDriveMode { OpenDrain = 0x1, } - #[repr(u8)] #[allow(dead_code)] #[derive(Clone, Copy, Debug)] @@ -201,7 +200,6 @@ pub enum InterruptMode { MappedToI3cIbi = 0x3, } - #[repr(u8)] #[allow(dead_code)] #[derive(Clone, Copy, Debug)] @@ -224,16 +222,14 @@ pub enum GyroMode { ContinuousHighPerformance = 0x7, } - const BMI323_CHIP_ID: u16 = 0x0043; const READ_BIT: u8 = 0x80; -impl<'a, 'buf> - Bmi323<'a, 'buf> { +impl<'a, 'buf> Bmi323<'a, 'buf> { /// creates a new BMI323 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, Async>, - spi_cs: Output<'a>, + spi: spi::Spi<'a, Async>, + spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { Bmi323 { @@ -269,18 +265,12 @@ impl<'a, 'buf> spi_config.frequency = hz(1_000_000); let imu_spi = spi::Spi::new( - peri, - sck_pin, - mosi_pin, - miso_pin, - tx_dma, - rx_dma, - spi_config + peri, sck_pin, mosi_pin, miso_pin, tx_dma, rx_dma, spi_config, ); let spi_cs = Output::new(spi_cs_pin, Level::High, Speed::VeryHigh); - Bmi323 { + Bmi323 { spi: imu_spi, spi_cs, spi_buf, @@ -321,14 +311,17 @@ impl<'a, 'buf> async fn burst_read(&mut self, reg: ImuRegisters, dest: &mut [u16]) { self.select(); - // the transaction length is either the dest buf size * 2 + 2 + // the transaction length is either the dest buf size * 2 + 2 // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) // OR upper bounded by internal length of the buffer. let trx_len = min((dest.len() * 2) + 2, self.spi_buf.len()); self.spi_buf[0] = reg as u8 | READ_BIT; self.spi_buf[1] = 0x00; - let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + let _ = self + .spi + .transfer_in_place(&mut self.spi_buf[..trx_len]) + .await; for (i, dword) in dest.iter_mut().enumerate() { *dword = (self.spi_buf[(i * 2) + 2 + 1] as u16) << 8 | self.spi_buf[(i * 2) + 2] as u16; @@ -352,7 +345,7 @@ impl<'a, 'buf> async fn burst_write(&mut self, reg: ImuRegisters, data: &[u16]) { self.select(); - // the transaction length is either the dest buf size * 2 + 2 + // the transaction length is either the dest buf size * 2 + 2 // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) // OR upper bounded by internal length of the buffer. let trx_len = min((data.len() * 2) + 1, self.spi_buf.len()); @@ -364,7 +357,10 @@ impl<'a, 'buf> self.spi_buf[(i * 2) + 1 + 1] = ((*word & 0xFF00_u16) >> 8) as u8; } - let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + let _ = self + .spi + .transfer_in_place(&mut self.spi_buf[..trx_len]) + .await; self.deselect(); } @@ -399,12 +395,16 @@ impl<'a, 'buf> let _ = self.read(ImuRegisters::CHIP_ID).await; let chip_id = self.read(ImuRegisters::CHIP_ID).await & 0x00FF; if chip_id != BMI323_CHIP_ID { - defmt::warn!("read IMU ID (0x{:x}) does not match expected BMI323 ID (0x{:x})", chip_id, BMI323_CHIP_ID); + defmt::warn!( + "read IMU ID (0x{:x}) does not match expected BMI323 ID (0x{:x})", + chip_id, + BMI323_CHIP_ID + ); has_self_test_error = Err(()); } else { defmt::debug!("BMI323 id verified: 0x{:x}", chip_id); } - + if (self.accel_self_test().await).is_err() { has_self_test_error = Err(()); } @@ -415,7 +415,7 @@ impl<'a, 'buf> has_self_test_error } - + pub async fn accel_get_raw_data(&mut self) -> [i16; 3] { let mut buf: [u16; 3] = [0; 3]; self.burst_read(ImuRegisters::ACC_DATA_X, &mut buf).await; @@ -443,17 +443,21 @@ impl<'a, 'buf> pub async fn accel_get_data_g(&mut self) -> [f32; 3] { let raw_data = self.accel_get_raw_data().await; - [self.convert_accel_raw_sample_g(raw_data[0]), + [ + self.convert_accel_raw_sample_g(raw_data[0]), self.convert_accel_raw_sample_g(raw_data[1]), - self.convert_accel_raw_sample_g(raw_data[2])] + self.convert_accel_raw_sample_g(raw_data[2]), + ] } pub async fn accel_get_data_mps(&mut self) -> [f32; 3] { let raw_data = self.accel_get_raw_data().await; - [self.convert_accel_raw_sample_mps(raw_data[0]), + [ + self.convert_accel_raw_sample_mps(raw_data[0]), self.convert_accel_raw_sample_mps(raw_data[1]), - self.convert_accel_raw_sample_mps(raw_data[2])] + self.convert_accel_raw_sample_mps(raw_data[2]), + ] } pub async fn apply_accel_config(&mut self) -> Result<(), ()> { @@ -464,23 +468,26 @@ impl<'a, 'buf> new_acc_conf_reg_val |= (self.accel_range as u16 & 0x07) << 4; new_acc_conf_reg_val |= self.accel_odr as u16 & 0x0F; - self.write(ImuRegisters::ACC_CONF, new_acc_conf_reg_val).await; + self.write(ImuRegisters::ACC_CONF, new_acc_conf_reg_val) + .await; let err_reg_val = self.read(ImuRegisters::ERR_REG).await; if err_reg_val & 0x0020 != 0 { defmt::error!("BMI323 accel config is invalid. Accel may be inop."); - return Err(()) + return Err(()); } Ok(()) } - pub async fn set_accel_config(&mut self, + pub async fn set_accel_config( + &mut self, accel_mode: AccelMode, accel_range: AccelRange, accel_bw_mode: Bandwidth3DbCutoffFreq, accel_odr: OutputDataRate, - accel_avg_window: DataAveragingWindow) -> Result<(), ()> { + accel_avg_window: DataAveragingWindow, + ) -> Result<(), ()> { self.accel_mode = accel_mode; self.accel_range = accel_range; self.accel_bw_mode = accel_bw_mode; @@ -532,17 +539,21 @@ impl<'a, 'buf> pub async fn gyro_get_data_dps(&mut self) -> [f32; 3] { let raw_data = self.gyro_get_raw_data().await; - [self.convert_raw_gyro_sample_dps(raw_data[0]), + [ + self.convert_raw_gyro_sample_dps(raw_data[0]), self.convert_raw_gyro_sample_dps(raw_data[1]), - self.convert_raw_gyro_sample_dps(raw_data[2])] + self.convert_raw_gyro_sample_dps(raw_data[2]), + ] } pub async fn gyro_get_data_rads(&mut self) -> [f32; 3] { let raw_data = self.gyro_get_raw_data().await; - [self.convert_raw_gyro_sample_rads(raw_data[0]), + [ + self.convert_raw_gyro_sample_rads(raw_data[0]), self.convert_raw_gyro_sample_rads(raw_data[1]), - self.convert_raw_gyro_sample_rads(raw_data[2])] + self.convert_raw_gyro_sample_rads(raw_data[2]), + ] } pub async fn apply_gyro_config(&mut self) -> Result<(), ()> { @@ -553,23 +564,26 @@ impl<'a, 'buf> new_gyr_conf_reg_val |= (self.gyro_range as u16 & 0x07) << 4; new_gyr_conf_reg_val |= self.gyro_odr as u16 & 0x0F; - self.write(ImuRegisters::GYR_CONF, new_gyr_conf_reg_val).await; + self.write(ImuRegisters::GYR_CONF, new_gyr_conf_reg_val) + .await; let err_reg_val = self.read(ImuRegisters::ERR_REG).await; if err_reg_val & 0x0040 != 0 { defmt::error!("BMI323 gyro config is invalid. Gyro may be inop."); - return Err(()) + return Err(()); } Ok(()) } - pub async fn set_gyro_config(&mut self, + pub async fn set_gyro_config( + &mut self, gyro_mode: GyroMode, gyro_range: GyroRange, gyro_bw_mode: Bandwidth3DbCutoffFreq, gyro_odr: OutputDataRate, - gyro_avg_window: DataAveragingWindow) -> Result<(), ()> { + gyro_avg_window: DataAveragingWindow, + ) -> Result<(), ()> { self.gyro_mode = gyro_mode; self.gyro_range = gyro_range; self.gyro_bw_mode = gyro_bw_mode; @@ -605,7 +619,11 @@ impl<'a, 'buf> self.write(ImuRegisters::INT_MAP2, int_map2_reg_val).await; } - pub async fn set_int1_pin_config(&mut self, pin_level: IntPinLevel, pin_drive_mode: IntPinDriveMode) { + pub async fn set_int1_pin_config( + &mut self, + pin_level: IntPinLevel, + pin_drive_mode: IntPinDriveMode, + ) { let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; // clear the config for int1 pin @@ -614,10 +632,15 @@ impl<'a, 'buf> io_int_ctrl_reg_val |= pin_level as u16 & 0x1; io_int_ctrl_reg_val |= (pin_drive_mode as u16 & 0x1) << 1; - self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val) + .await; } - pub async fn set_int2_pin_config(&mut self, pin_level: IntPinLevel, pin_drive_mode: IntPinDriveMode) { + pub async fn set_int2_pin_config( + &mut self, + pin_level: IntPinLevel, + pin_drive_mode: IntPinDriveMode, + ) { let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; // clear the config for int1 pin @@ -626,7 +649,8 @@ impl<'a, 'buf> io_int_ctrl_reg_val |= (pin_level as u16 & 0x1) << 8; io_int_ctrl_reg_val |= (pin_drive_mode as u16 & 0x1) << 9; - self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val) + .await; } pub async fn set_int1_enabled(&mut self, enabled: bool) { @@ -637,7 +661,8 @@ impl<'a, 'buf> // set the new mapping for accel io_int_ctrl_reg_val |= (enabled as u16 & 0x1) << 2; - self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val) + .await; } pub async fn set_int2_enabled(&mut self, enabled: bool) { @@ -648,6 +673,7 @@ impl<'a, 'buf> // set the new mapping for accel io_int_ctrl_reg_val |= (enabled as u16 & 0x1) << 10; - self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val) + .await; } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/imu/mod.rs b/lib-stm32/src/drivers/imu/mod.rs index d1d87e2c..2e786535 100644 --- a/lib-stm32/src/drivers/imu/mod.rs +++ b/lib-stm32/src/drivers/imu/mod.rs @@ -1,3 +1,2 @@ pub mod bmi085; pub mod bmi323; - diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index a0ac7506..20ad800f 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -1,7 +1,11 @@ - use core::ops::Range; -use embassy_stm32::{mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, time::mhz, Peripheral}; +use embassy_stm32::{ + mode::Async, + spi::{self, Config, MosiPin, SckPin, Spi}, + time::mhz, + Peripheral, +}; use smart_leds::RGB8; use crate::anim::{AnimInterface, CompositeAnimation}; @@ -18,16 +22,20 @@ pub const fn apa102_buf_len(num_leds: usize) -> usize { } pub struct Apa102<'a, 'buf, const NUM_LEDS: usize> -where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { +where + [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]:, +{ spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], needs_update: bool, } -impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> -where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { +impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> +where + [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]:, +{ pub fn new( - spi: spi::Spi<'a, Async>, + spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], ) -> Self { // set start frame @@ -42,7 +50,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 2] = 0xFF; spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 1] = 0xFF; - Apa102 { + Apa102 { spi, spi_buf, needs_update: false, @@ -59,13 +67,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { let mut dotstar_spi_config = Config::default(); dotstar_spi_config.frequency = mhz(1); - let spi = Spi::new_txonly( - peri, - sck_pin, - mosi_pin, - tx_dma, - dotstar_spi_config, - ); + let spi = Spi::new_txonly(peri, sck_pin, mosi_pin, tx_dma, dotstar_spi_config); Self::new(spi, spi_buf) } @@ -110,9 +112,10 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { pub fn set_color(&mut self, color: RGB8, led_index: usize) { assert!(led_index < NUM_LEDS); - if self.spi_buf[Self::l2r(led_index)] != color.r - || self.spi_buf[Self::l2g(led_index)] != color.g - || self.spi_buf[Self::l2b(led_index)] != color.b { + if self.spi_buf[Self::l2r(led_index)] != color.r + || self.spi_buf[Self::l2g(led_index)] != color.g + || self.spi_buf[Self::l2b(led_index)] != color.b + { self.needs_update = true; } @@ -139,8 +142,10 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { } } -pub struct Apa102Anim<'a, 'buf, 'ca, const NUM_LEDS: usize> -where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { +pub struct Apa102Anim<'a, 'buf, 'ca, const NUM_LEDS: usize> +where + [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]:, +{ apa102_driver: Apa102<'a, 'buf, NUM_LEDS>, active_animation: [usize; NUM_LEDS], animation_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS], @@ -148,9 +153,14 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { // animation_buf: [Option<&'ca mut CompositeAnimation<'ca, u8, RGB8>>; NUM_LEDS], } -impl<'a, 'buf, 'ca, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, NUM_LEDS> -where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { - pub fn new(apa102: Apa102<'a, 'buf, NUM_LEDS>, anim_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS]) -> Self { +impl<'a, 'buf, 'ca, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, NUM_LEDS> +where + [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]:, +{ + pub fn new( + apa102: Apa102<'a, 'buf, NUM_LEDS>, + anim_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS], + ) -> Self { Apa102Anim { apa102_driver: apa102, active_animation: [0; NUM_LEDS], @@ -158,11 +168,14 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { } } - pub fn next_valid_anim(search_start_ind: usize, anim_playbook: &[CompositeAnimation<'ca, u8, RGB8>]) -> usize { + pub fn next_valid_anim( + search_start_ind: usize, + anim_playbook: &[CompositeAnimation<'ca, u8, RGB8>], + ) -> usize { if anim_playbook.len() == 1 { return 0; } - + let mut cur_index = search_start_ind; loop { // inc and loop the index @@ -210,7 +223,8 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { anim_playbook[cur_active_anim].reset_animation(); // select and start the next animation - let next_active_anim = Self::next_valid_anim(cur_active_anim, anim_playbook); + let next_active_anim = + Self::next_valid_anim(cur_active_anim, anim_playbook); anim_playbook[next_active_anim].start_animation(); self.active_animation[led_index] = next_active_anim; @@ -229,9 +243,14 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { self.apa102_driver.update().await; } - fn set_animation_enabled(&mut self, led_index: usize, anim_id: usize, enable: bool) -> Result<(), LedChainError> { + fn set_animation_enabled( + &mut self, + led_index: usize, + anim_id: usize, + enable: bool, + ) -> Result<(), LedChainError> { if led_index >= NUM_LEDS { - return Err(LedChainError::InvalidLedIndex) + return Err(LedChainError::InvalidLedIndex); } if let Some(animation_playbook) = self.animation_playbook_buf[led_index].as_deref_mut() { @@ -254,11 +273,19 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { Ok(()) } - pub fn enable_animation(&mut self, led_index: usize, anim_id: usize) -> Result<(), LedChainError> { + pub fn enable_animation( + &mut self, + led_index: usize, + anim_id: usize, + ) -> Result<(), LedChainError> { self.set_animation_enabled(led_index, anim_id, true) } - pub fn disable_animation(&mut self, led_index: usize, anim_id: usize) -> Result<(), LedChainError> { + pub fn disable_animation( + &mut self, + led_index: usize, + anim_id: usize, + ) -> Result<(), LedChainError> { self.set_animation_enabled(led_index, anim_id, false) } @@ -286,4 +313,3 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { self.apa102_driver.set_color_all(color); } } - diff --git a/lib-stm32/src/drivers/led/mod.rs b/lib-stm32/src/drivers/led/mod.rs index d7aa8cfd..13a2d3b0 100644 --- a/lib-stm32/src/drivers/led/mod.rs +++ b/lib-stm32/src/drivers/led/mod.rs @@ -2,5 +2,5 @@ pub mod apa102; #[derive(Debug)] pub enum LedChainError { - InvalidLedIndex -} \ No newline at end of file + InvalidLedIndex, +} diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index b4339e11..cfdcbb31 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -4,6 +4,6 @@ pub mod boot; pub mod flash; pub mod imu; pub mod led; +pub mod other; pub mod radio; pub mod switches; -pub mod other; \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index ea300dda..3c680a04 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -1,4 +1,3 @@ - use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime}; use embassy_stm32::Peripheral; @@ -12,24 +11,18 @@ pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { adc_bins: u32, } -impl< - 'a, - T: adc::Instance, - Ch: AdcChannel> - AdcHelper<'a, T, Ch> { - - // NOTE: vref_int_peri is not checked by compiler and needs to +impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> { + // NOTE: vref_int_peri is not checked by compiler and needs to // be the peripheral connected to Vref_int. pub fn new( peri: impl Peripheral

+ 'a, pin: Ch, sample_time: SampleTime, - resolution: Resolution - ) -> Self { - + resolution: Resolution, + ) -> Self { let mut adc_inst = Adc::new(peri); - adc_inst.set_sample_time(sample_time); + adc_inst.set_sample_time(sample_time); adc_inst.set_resolution(resolution); // Use resolution to calculate the max ADC min quantity.' @@ -43,7 +36,7 @@ impl< AdcHelper { inst: adc_inst, pin, - adc_bins + adc_bins, } } @@ -59,6 +52,7 @@ impl< // defmt::info!("vref_int_cal: {}", vref_int_cal); // defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); - V_CAL_V * (self.inst.blocking_read(&mut self.pin) as f32) / (self.adc_bins as f32)// * vref_int_cal / vref_int_read_mv; + V_CAL_V * (self.inst.blocking_read(&mut self.pin) as f32) / (self.adc_bins as f32) + // * vref_int_cal / vref_int_read_mv; } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/other/mod.rs b/lib-stm32/src/drivers/other/mod.rs index 1e9398cf..0127ec29 100644 --- a/lib-stm32/src/drivers/other/mod.rs +++ b/lib-stm32/src/drivers/other/mod.rs @@ -1 +1 @@ -pub mod adc_helper; \ No newline at end of file +pub mod adc_helper; diff --git a/lib-stm32/src/drivers/radio/at_protocol.rs b/lib-stm32/src/drivers/radio/at_protocol.rs index c65d092d..4f8153a5 100644 --- a/lib-stm32/src/drivers/radio/at_protocol.rs +++ b/lib-stm32/src/drivers/radio/at_protocol.rs @@ -214,8 +214,14 @@ impl ATEvent<'_> { match event { Self::PEER_CONNECTED => { - let peer_handle = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let type_ = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?; + let peer_handle = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let type_ = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)?; match type_ { Self::PEER_CONNECTED_IPV4 | Self::PEER_CONNECTED_IPV6 => { let protocol = params @@ -223,11 +229,24 @@ impl ATEvent<'_> { .ok_or(AtPacketError::TypeDecodeParameterMissing)? .parse::() .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))? - .try_into().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let local_address = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?; - let local_port = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let remote_address = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?; - let remote_port = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + .try_into() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let local_address = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)?; + let local_port = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let remote_address = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)?; + let remote_port = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::PeerConnectedIP { peer_handle, @@ -244,13 +263,27 @@ impl ATEvent<'_> { } } Self::PEER_DISCONNECTED => { - let peer_handle = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let peer_handle = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::PeerDisconnected { peer_handle }) } Self::WIFI_LINK_CONNECTED => { - let conn_id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let bssid = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?; - let channel = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let conn_id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let bssid = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)?; + let channel = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::WifiLinkConnected { conn_id, @@ -259,50 +292,89 @@ impl ATEvent<'_> { }) } Self::WIFI_LINK_DISCONNECTED => { - let conn_id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let conn_id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; let reason = params .next() .ok_or(AtPacketError::TypeDecodeParameterMissing)? .parse::() .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))? - .try_into().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + .try_into() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::WifiLinkDisconnected { conn_id, reason }) } Self::WIFI_ACCESS_POINT_UP => { - let id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::WifiAccessPointUp { id }) } Self::WIFI_ACCESS_POINT_DOWN => { - let id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::WifiAccessPointDown { id }) } Self::WIFI_AP_STATION_CONNECTED => { - let id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let mac_addr = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?; + let id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let mac_addr = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)?; Ok(ATEvent::WifiAccessPointStationConnected { id, mac_addr }) } Self::WIFI_AP_STATION_DISCONNECTED => { - let id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::WifiAccessPointStationDisconnected { id }) } Self::ETHERNET_LINK_UP => Ok(ATEvent::EthernetLinkUp), Self::ETHERNET_LINK_DOWN => Ok(ATEvent::EthernetLinkDown), Self::NETWORK_UP => { - let interface_id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let interface_id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::NetworkUp { interface_id }) } Self::NETWORK_DOWN => { - let interface_id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let interface_id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::NetworkDown { interface_id }) } Self::NETWORK_ERROR => { - let interface_id = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; - let code = params.next().ok_or(AtPacketError::TypeDecodeParameterMissing)?.parse::().or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let interface_id = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; + let code = params + .next() + .ok_or(AtPacketError::TypeDecodeParameterMissing)? + .parse::() + .or(Err(AtPacketError::TypeDecodeParameterDataTypeInvalid))?; Ok(ATEvent::NetworkError { interface_id, code }) } @@ -327,8 +399,10 @@ impl ATResponse<'_> { pub fn new(buf: &[u8]) -> Result, AtPacketError> { // TODO: error handling in this function is bad let s = core::str::from_utf8(buf).or(Err(AtPacketError::Utf8DecodeFailed))?; - - let i_echo = s.find(Self::CR_LF).ok_or(AtPacketError::FramingDecodeFailed)?; + + let i_echo = s + .find(Self::CR_LF) + .ok_or(AtPacketError::FramingDecodeFailed)?; let s = &s[i_echo + Self::CR_LF.len()..]; if !s.ends_with(Self::CR_LF) { diff --git a/lib-stm32/src/drivers/radio/edm_protocol.rs b/lib-stm32/src/drivers/radio/edm_protocol.rs index a2db69e0..60198a94 100644 --- a/lib-stm32/src/drivers/radio/edm_protocol.rs +++ b/lib-stm32/src/drivers/radio/edm_protocol.rs @@ -138,7 +138,8 @@ impl EdmPacketRaw { }) } Self::AT_REQUEST => Ok(EdmPacket::ATRequest( - core::str::from_utf8(&self.payload).or(Err(EdmPacketError::AtRequestEventDecodeError))?, + core::str::from_utf8(&self.payload) + .or(Err(EdmPacketError::AtRequestEventDecodeError))?, )), Self::AT_RESPONSE => Ok(EdmPacket::ATResponse(ATResponse::new(&self.payload)?)), Self::AT_EVENT => Ok(EdmPacket::ATEvent(ATEvent::new(&self.payload)?)), diff --git a/lib-stm32/src/drivers/radio/mod.rs b/lib-stm32/src/drivers/radio/mod.rs index 257e966b..ae95379e 100644 --- a/lib-stm32/src/drivers/radio/mod.rs +++ b/lib-stm32/src/drivers/radio/mod.rs @@ -1,3 +1,3 @@ pub mod at_protocol; pub mod edm_protocol; -pub mod odin_w26x; \ No newline at end of file +pub mod odin_w26x; diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 7576dcd8..cda0793c 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -199,7 +199,8 @@ impl< pub async fn set_host_name(&self, host_name: &str) -> Result<(), OdinRadioError> { let mut str: String<64> = String::new(); - write!(str, "AT+UNHN=\"{host_name}\"").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UNHN=\"{host_name}\"") + .or(Err(OdinRadioError::CommandConstructionFailed))?; defmt::trace!("host configuration string: {}", str.as_str()); self.send_command(str.as_str()).await?; defmt::trace!("sent configuration command"); @@ -216,22 +217,26 @@ impl< auth: WifiAuth<'_>, ) -> Result<(), OdinRadioError> { let mut str: String<64> = String::new(); - write!(str, "AT+UWSC={config_id},2,\"{ssid}\"").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UWSC={config_id},2,\"{ssid}\"") + .or(Err(OdinRadioError::CommandConstructionFailed))?; self.send_command(str.as_str()).await?; self.read_ok().await?; str.clear(); match auth { WifiAuth::Open => { - write!(str, "AT+UWSC={config_id},5,1").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UWSC={config_id},5,1") + .or(Err(OdinRadioError::CommandConstructionFailed))?; self.send_command(str.as_str()).await?; self.read_ok().await?; } WifiAuth::WPA { passphrase } => { - write!(str, "AT+UWSC={config_id},5,2").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UWSC={config_id},5,2") + .or(Err(OdinRadioError::CommandConstructionFailed))?; self.send_command(str.as_str()).await?; self.read_ok().await?; str.clear(); - write!(str, "AT+UWSC={config_id},8,\"{passphrase}\"").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UWSC={config_id},8,\"{passphrase}\"") + .or(Err(OdinRadioError::CommandConstructionFailed))?; self.send_command(str.as_str()).await?; self.read_ok().await?; } @@ -264,36 +269,47 @@ impl< async move { let mut run = true; while run { - let _ = self.reader.dequeue(|buf| { - // defmt::warn!("buf {}", buf); - let packet = self.to_packet(buf); - if packet.is_err() { - defmt::warn!("parsed invalid packet"); - } else { - let packet = packet.unwrap(); - if let EdmPacket::ATEvent(ATEvent::NetworkDown { interface_id: 0 }) = packet { - defmt::debug!("got network down event."); - run = false; - } else if let EdmPacket::ATEvent(ATEvent::WifiLinkDisconnected { conn_id:_, reason: WifiLinkDisconnectedReason::NetworkDisabled }) = packet { - run = false; - } else if let EdmPacket::ATResponse(ATResponse::Ok(_)) = packet { - run = false; + let _ = self + .reader + .dequeue(|buf| { + // defmt::warn!("buf {}", buf); + let packet = self.to_packet(buf); + if packet.is_err() { + defmt::warn!("parsed invalid packet"); } else { - defmt::warn!("got edm packet: {}", packet); + let packet = packet.unwrap(); + if let EdmPacket::ATEvent(ATEvent::NetworkDown { + interface_id: 0, + }) = packet + { + defmt::debug!("got network down event."); + run = false; + } else if let EdmPacket::ATEvent(ATEvent::WifiLinkDisconnected { + conn_id: _, + reason: WifiLinkDisconnectedReason::NetworkDisabled, + }) = packet + { + run = false; + } else if let EdmPacket::ATResponse(ATResponse::Ok(_)) = packet { + run = false; + } else { + defmt::warn!("got edm packet: {}", packet); + } } - } - }).await; + }) + .await; } }, - Timer::after_millis(2500)).await { - embassy_futures::select::Either::First(_) => { - Ok(()) - }, - embassy_futures::select::Either::Second(_) => { - defmt::warn!("disconnect timed out"); - Err(OdinRadioError::OperationTimedOut) - }, + Timer::after_millis(2500), + ) + .await + { + embassy_futures::select::Either::First(_) => Ok(()), + embassy_futures::select::Either::Second(_) => { + defmt::warn!("disconnect timed out"); + Err(OdinRadioError::OperationTimedOut) } + } } pub async fn connect_wifi(&self, config_id: u8) -> Result<(), OdinRadioError> { @@ -339,7 +355,8 @@ impl< ) -> Result<(), OdinRadioError> { let mut str: String<64> = String::new(); let server_type = server_type as u8; - write!(str, "AT+UDSC={server_id},{server_type},{port},1,0").or(Err(OdinRadioError::CommandConstructionFailed))?; + write!(str, "AT+UDSC={server_id},{server_type},{port},1,0") + .or(Err(OdinRadioError::CommandConstructionFailed))?; self.send_command(str.as_str()).await?; self.read_ok().await?; Ok(()) @@ -389,7 +406,9 @@ impl< // defmt::info!("AT resp connect event"); if let Some(i) = resp.find("+UDCP:") { - peer_id = Some(resp[i + 6..].parse::().or(Err(OdinRadioError::PeerConnectionReceivedInvalidResponse))?); + peer_id = Some(resp[i + 6..].parse::().or(Err( + OdinRadioError::PeerConnectionReceivedInvalidResponse, + ))?); } else { return Err(OdinRadioError::PeerConnectionFailed); } @@ -450,12 +469,13 @@ impl< pub fn send_data(&self, channel_id: u8, data: &[u8]) -> Result<(), OdinRadioError> { let res = self.writer.enqueue(|buf| { - EdmPacket::DataCommand { - channel: channel_id, - data, - } - .write(buf).unwrap() - }); + EdmPacket::DataCommand { + channel: channel_id, + data, + } + .write(buf) + .unwrap() + }); if res.is_err() { // queue was full @@ -476,7 +496,8 @@ impl< } else { Err(OdinRadioError::ReadDataInvalid) } - }).await + }) + .await } pub fn can_read_data(&'a self) -> bool { @@ -484,27 +505,27 @@ impl< } pub fn try_read_data(&'a self, fn_read: FN) -> Result - where FN: FnOnce(&[u8]) -> RET, + where + FN: FnOnce(&[u8]) -> RET, { match self.reader.try_dequeue() { Ok(buf) => { match self.to_packet(buf.data()) { Ok(pkt) => { - if let EdmPacket::DataEvent { channel: _ , data } = pkt { + if let EdmPacket::DataEvent { channel: _, data } = pkt { Ok(fn_read(data)) } else { // defmt::trace!("got non data event"); Err(OdinRadioError::ReadDataInvalid) } - }, + } Err(_) => { // defmt::trace!("got data that wasn't an edm packet: {}", buf.data()); Err(OdinRadioError::ReadDataInvalid) - }, + } } // we read something - - }, + } Err(queue::Error::QueueFull) => { // nothing to read return Err(OdinRadioError::ReadLowLevelBufferEmpty); @@ -516,7 +537,7 @@ impl< Err(queue::Error::InProgress) => { // you did something illegal Err(OdinRadioError::ReadLowLevelBufferBusy) - }, + } } } @@ -524,24 +545,26 @@ impl< match self.mode { RadioMode::CommandMode => { let res = self.writer.enqueue(|buf| { - buf[..cmd.len()].clone_from_slice(cmd.as_bytes()); - buf[cmd.len()] = b'\r'; - cmd.len() + 1 - }); + buf[..cmd.len()].clone_from_slice(cmd.as_bytes()); + buf[cmd.len()] = b'\r'; + cmd.len() + 1 + }); if res.is_err() { // queue was full - return Err(OdinRadioError::SendCommandLowLevelBufferFull) + return Err(OdinRadioError::SendCommandLowLevelBufferFull); } Ok(()) } RadioMode::ExtendedDataMode => { - let res = self.writer.enqueue(|buf| EdmPacket::ATRequest(cmd).write(buf).unwrap()); + let res = self + .writer + .enqueue(|buf| EdmPacket::ATRequest(cmd).write(buf).unwrap()); if res.is_err() { // queue was full - return Err(OdinRadioError::SendCommandLowLevelBufferFull) + return Err(OdinRadioError::SendCommandLowLevelBufferFull); } Ok(()) @@ -620,7 +643,6 @@ impl< if brk { break; } - } res @@ -628,17 +650,20 @@ impl< async fn read_ok_at_edm_transition(&self) -> Result { let transition_buf: [u8; 12] = [13, 10, 79, 75, 13, 10, 170, 0, 2, 0, 113, 85]; - - - self.reader.dequeue(|buf| { - if buf.len() == transition_buf.len() && buf.iter().zip(transition_buf.iter()).all(|(a,b)| a == b) { - Ok(true) - } else if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { - Ok(false) - } else { - Err(OdinRadioError::EdmTransitionFailed) - } - }).await + + self.reader + .dequeue(|buf| { + if buf.len() == transition_buf.len() + && buf.iter().zip(transition_buf.iter()).all(|(a, b)| a == b) + { + Ok(true) + } else if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { + Ok(false) + } else { + Err(OdinRadioError::EdmTransitionFailed) + } + }) + .await } fn to_packet<'b>(&self, buf: &'b [u8]) -> Result, EdmPacketError> { diff --git a/lib-stm32/src/drivers/switches/button.rs b/lib-stm32/src/drivers/switches/button.rs index 16d37405..ea315559 100644 --- a/lib-stm32/src/drivers/switches/button.rs +++ b/lib-stm32/src/drivers/switches/button.rs @@ -1,6 +1,10 @@ use defmt::Format; use embassy_futures::select; -use embassy_stm32::{exti::ExtiInput, gpio::{Pin, Pull}, Peripheral}; +use embassy_stm32::{ + exti::ExtiInput, + gpio::{Pin, Pull}, + Peripheral, +}; use embassy_time::{Instant, Timer}; #[derive(Clone, Copy, Eq, PartialEq, Debug, Format)] @@ -11,7 +15,11 @@ pub enum AdvButtonEvent { Held, } -pub const ADV_BTN_EVENT_DOUBLE_TAP: [AdvButtonEvent; 3] = [AdvButtonEvent::ShortPress, AdvButtonEvent::ShortPress, AdvButtonEvent::None]; +pub const ADV_BTN_EVENT_DOUBLE_TAP: [AdvButtonEvent; 3] = [ + AdvButtonEvent::ShortPress, + AdvButtonEvent::ShortPress, + AdvButtonEvent::None, +]; #[derive(Clone, Copy, Debug)] enum BtnState { @@ -19,7 +27,12 @@ enum BtnState { Released(Instant), } -pub struct AdvExtiButton { +pub struct AdvExtiButton< + const SHORT_PRESS_TIME_MS: u64 = 300, + const LONG_PRESS_TIME_MS: u64 = 600, + const HOLD_PRESS_TIME_MS: u64 = 1200, + const PRESS_TO_MS: u64 = 400, +> { input: ExtiInput<'static>, input_inverted: bool, @@ -28,7 +41,13 @@ pub struct AdvExtiButton AdvExtiButton { +impl< + const SHORT_PRESS_TIME_MS: u64, + const LONG_PRESS_TIME_MS: u64, + const HOLD_PRESS_TIME_MS: u64, + const PRESS_TO_MS: u64, + > AdvExtiButton +{ pub fn new(input: ExtiInput<'static>, input_inverted: bool) -> Self { Self { input, @@ -39,7 +58,11 @@ impl(input_pin: PIN, input_pin_exti: impl Peripheral

::ExtiChannel> + 'static, input_inverted: bool) -> Self { + pub fn new_from_pins( + input_pin: PIN, + input_pin_exti: impl Peripheral

::ExtiChannel> + 'static, + input_inverted: bool, + ) -> Self { let input = ExtiInput::new(input_pin, input_pin_exti, Pull::None); Self::new(input, input_inverted) } @@ -75,15 +98,17 @@ impl { // btn state didn't change - }, + } (BtnState::Pressed(pressed_time), false) => { let btn_held_time = (now - pressed_time).as_millis(); // 50 ms debounce - if 50 <= btn_held_time { + if 50 <= btn_held_time { if btn_held_time <= SHORT_PRESS_TIME_MS { self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::ShortPress; self.btn_event_ind += 1; - } else if SHORT_PRESS_TIME_MS <= btn_held_time && btn_held_time <= LONG_PRESS_TIME_MS { + } else if SHORT_PRESS_TIME_MS <= btn_held_time + && btn_held_time <= LONG_PRESS_TIME_MS + { self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::LongPress; self.btn_event_ind += 1; } else if HOLD_PRESS_TIME_MS <= btn_held_time { @@ -103,7 +128,7 @@ impl { let btn_rel_time = (now - released_time).as_millis(); @@ -111,7 +136,7 @@ impl { // btn state didn't change let btn_rel_time = (now - released_time).as_millis(); @@ -125,15 +150,14 @@ impl { self.wait_for_release().await; @@ -141,11 +165,13 @@ impl { - match select::select(self.wait_for_press(), Timer::after_millis(PRESS_TO_MS)).await { + match select::select(self.wait_for_press(), Timer::after_millis(PRESS_TO_MS)) + .await + { select::Either::First(_) => { let now = Instant::now(); let btn_rel_time = (now - prev_rel_time).as_millis(); - + // 50 ms debounce if 50 <= btn_rel_time { self.prev_btn_state = BtnState::Pressed(now); } - }, + } select::Either::Second(_) => { // timed out waiting for a press, reset the buffer self.btn_event_buf = [AdvButtonEvent::None; 3]; self.btn_event_ind = 0; - }, + } } - }, - } + } + } } } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs index 6eae51fa..66416ac0 100644 --- a/lib-stm32/src/drivers/switches/dip.rs +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -1,29 +1,37 @@ -use core::{cmp::{max, min}, ops::Range}; +use core::{ + cmp::{max, min}, + ops::Range, +}; use embassy_stm32::gpio::{AnyPin, Input, Pull}; - - pub struct DipSwitch<'a, const PIN_CT: usize> { inputs: [Input<'a>; PIN_CT], - inversion_map: [bool; PIN_CT] + inversion_map: [bool; PIN_CT], } impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { - pub const fn new_from_inputs(inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { + pub const fn new_from_inputs( + inputs: [Input<'a>; PIN_CT], + inversion_map: Option<[bool; PIN_CT]>, + ) -> DipSwitch<'a, PIN_CT> { let inversion_map = if let Some(map) = inversion_map { map - } else { + } else { [false; PIN_CT] }; DipSwitch { inputs, - inversion_map + inversion_map, } } - pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { + pub fn new_from_pins( + pins: [AnyPin; PIN_CT], + pull: Pull, + inversion_map: Option<[bool; PIN_CT]>, + ) -> DipSwitch<'a, PIN_CT> { let inputs = pins.map(|pin| Input::new(pin, pull)); DipSwitch::new_from_inputs(inputs, inversion_map) } @@ -46,7 +54,7 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { // since pins are physical, we'll use an inclusive range let start_pin = min(pin_range.start, pin_range.end); - let end_pin = max(pin_range.start, pin_range.end) + 1; + let end_pin = max(pin_range.start, pin_range.end) + 1; let mut val: u8 = 0; for i in (start_pin..end_pin).enumerate() { @@ -55,4 +63,4 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { val } -} \ No newline at end of file +} diff --git a/lib-stm32/src/drivers/switches/mod.rs b/lib-stm32/src/drivers/switches/mod.rs index 817d5fe2..bcae84a1 100644 --- a/lib-stm32/src/drivers/switches/mod.rs +++ b/lib-stm32/src/drivers/switches/mod.rs @@ -1,3 +1,3 @@ pub mod button; pub mod dip; -pub mod rotary_encoder; \ No newline at end of file +pub mod rotary_encoder; diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs index f622d190..76b652a5 100644 --- a/lib-stm32/src/drivers/switches/rotary_encoder.rs +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -1,4 +1,3 @@ - use embassy_stm32::gpio::{AnyPin, Input, Pull}; pub struct RotaryEncoder<'a, const PIN_CT: usize> { @@ -7,10 +6,13 @@ pub struct RotaryEncoder<'a, const PIN_CT: usize> { } impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { - pub fn new_from_inputs(mut inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + pub fn new_from_inputs( + mut inputs: [Input<'a>; PIN_CT], + inversion_map: Option<[bool; PIN_CT]>, + ) -> RotaryEncoder<'a, PIN_CT> { let mut inversion_map = if let Some(map) = inversion_map { map - } else { + } else { [false; PIN_CT] }; @@ -23,7 +25,11 @@ impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { } } - pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + pub fn new_from_pins( + pins: [AnyPin; PIN_CT], + pull: Pull, + inversion_map: Option<[bool; PIN_CT]>, + ) -> RotaryEncoder<'a, PIN_CT> { let inputs = pins.map(|pin| Input::new(pin, pull)); RotaryEncoder::new_from_inputs(inputs, inversion_map) } @@ -42,4 +48,4 @@ impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { val } -} \ No newline at end of file +} diff --git a/lib-stm32/src/filter/mod.rs b/lib-stm32/src/filter/mod.rs index d8be14c2..c393ee41 100644 --- a/lib-stm32/src/filter/mod.rs +++ b/lib-stm32/src/filter/mod.rs @@ -12,14 +12,14 @@ pub trait Filter: Default { pub struct IirFilter { alpha: f32, - filtered_value: f32 + filtered_value: f32, } impl IirFilter { pub fn new(alpha: f32) -> Self { Self { alpha, - filtered_value: 0.0 + filtered_value: 0.0, } } @@ -51,7 +51,7 @@ impl Filter for IirFilter { } fn reset(&mut self) { - self.filtered_value = 0.0 + self.filtered_value = 0.0 } } @@ -62,24 +62,30 @@ pub struct WindowAvergingFilter WindowAvergingFilter { +impl + WindowAvergingFilter +{ pub fn new() -> Self { Self { window: [T::zero(); WINDOW_SIZE], update_ind: 0, filtered_value: T::zero(), - initialized: false + initialized: false, } } } -impl Default for WindowAvergingFilter { +impl Default + for WindowAvergingFilter +{ fn default() -> Self { Self::new() } } -impl Filter for WindowAvergingFilter { +impl Filter + for WindowAvergingFilter +{ fn add_sample(&mut self, sample: T) { // if we're configured for soft init // set every value to the first one @@ -93,7 +99,7 @@ impl Filter for W } self.window[self.update_ind] = sample; - + self.update_ind += 1; if self.update_ind >= WINDOW_SIZE { self.update_ind = 0; @@ -134,4 +140,4 @@ impl Filter for W self.update_ind = 0; self.initialized = false; } -} \ No newline at end of file +} diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 6dcc9443..13aff49b 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -1,8 +1,6 @@ #![no_std] - #![allow(incomplete_features)] -#![allow(clippy::too_many_arguments)] // too many functions passing pins to device drivers exceed the bound - +#![allow(clippy::too_many_arguments)] // too many functions passing pins to device drivers exceed the bound #![feature(generic_const_exprs)] #![feature(const_precise_live_drops)] #![feature(maybe_uninit_uninit_array)] @@ -21,13 +19,12 @@ pub mod audio; pub mod drivers; pub mod filter; pub mod math; -pub mod uart; -pub mod units; pub mod power; pub mod time; +pub mod uart; +pub mod units; pub mod queue; // required for exported queue macros pub extern crate paste; - diff --git a/lib-stm32/src/math/lerp.rs b/lib-stm32/src/math/lerp.rs index a1a83ffd..f9085132 100644 --- a/lib-stm32/src/math/lerp.rs +++ b/lib-stm32/src/math/lerp.rs @@ -15,17 +15,19 @@ pub trait LerpNumeric = Copy + Bounded + FromPrimitive; // return N::from_f32(lerp_f(a.to_f32().unwrap(), b.to_f32().unwrap(), t_pct)).unwrap(); // } -pub fn lerp(a: N, b: N, t: N) -> N -where N: FromPrimitive + Copy + Bounded, - f32: core::convert::From +pub fn lerp(a: N, b: N, t: N) -> N +where + N: FromPrimitive + Copy + Bounded, + f32: core::convert::From, { let t_pct = Into::::into(t) / Into::::into(N::max_value()); N::from_f32(f_lerp_f(Into::::into(a), Into::::into(b), t_pct)).unwrap() } -pub fn lerp_f(a: N, b: N, t: f32) -> N -where N: FromPrimitive + Copy + Bounded, - f32: core::convert::From +pub fn lerp_f(a: N, b: N, t: f32) -> N +where + N: FromPrimitive + Copy + Bounded, + f32: core::convert::From, { N::from_f32(f_lerp_f(Into::::into(a), Into::::into(b), t)).unwrap() } @@ -36,8 +38,9 @@ pub fn f_lerp_f(a: f32, b: f32, t: f32) -> f32 { } pub trait Lerp: Clone + Copy -where N: FromPrimitive + Copy + Bounded, - f32: core::convert::From +where + N: FromPrimitive + Copy + Bounded, + f32: core::convert::From, { fn lerp(a: Self, b: Self, t: N) -> Self; fn lerp_f(a: Self, b: Self, t: f32) -> Self; @@ -61,9 +64,10 @@ impl Lerp for RGB8 { } } -pub struct TimeLerp<'a, N, L: Lerp> -where N: FromPrimitive + Copy + Bounded, - f32: core::convert::From +pub struct TimeLerp<'a, N, L: Lerp> +where + N: FromPrimitive + Copy + Bounded, + f32: core::convert::From, { a: L, b: L, @@ -74,10 +78,11 @@ where N: FromPrimitive + Copy + Bounded, pd2: PhantomData<&'a N>, } -impl<'a, N, L> TimeLerp<'a, N, L> -where N: FromPrimitive + Copy + Bounded, -f32: core::convert::From, -L: Lerp +impl<'a, N, L> TimeLerp<'a, N, L> +where + N: FromPrimitive + Copy + Bounded, + f32: core::convert::From, + L: Lerp, { pub const fn new(a: L, b: L, duration: Duration) -> TimeLerp<'a, N, L> { TimeLerp { diff --git a/lib-stm32/src/math/linear_map.rs b/lib-stm32/src/math/linear_map.rs index a42e6243..c7f76ef9 100644 --- a/lib-stm32/src/math/linear_map.rs +++ b/lib-stm32/src/math/linear_map.rs @@ -5,7 +5,7 @@ use super::{range::Range, Number}; // Take input max and/or min (bounds), output value within new bounds pub struct LinearMap where - T: Number + T: Number, { input_range: Range, output_range: Range, @@ -13,16 +13,20 @@ where impl LinearMap where - T: Number + T: Number, { pub const fn new(input_range: Range, output_range: Range) -> Self { Self { input_range, - output_range + output_range, } } - pub fn map_ranges_bounded(val: N, input_range: Range, output_range: Range) -> N { + pub fn map_ranges_bounded( + val: N, + input_range: Range, + output_range: Range, + ) -> N { let clamped_val = clamp_min(clamp_max(val, input_range.max()), input_range.min()); input_range.map_value_to_range(clamped_val, &output_range) } @@ -32,9 +36,11 @@ where } pub fn map_boudned(&self, val: T) -> T { - let val = clamp_min(clamp_max(val, self.input_range.max()), self.input_range.min()); - + let val = clamp_min( + clamp_max(val, self.input_range.max()), + self.input_range.min(), + ); + self.map(val) } } - diff --git a/lib-stm32/src/math/mod.rs b/lib-stm32/src/math/mod.rs index 41b5cb17..db26864f 100644 --- a/lib-stm32/src/math/mod.rs +++ b/lib-stm32/src/math/mod.rs @@ -1,13 +1,19 @@ use num_traits::{FromPrimitive, Num, ToPrimitive}; -pub mod linear_map; pub mod lerp; +pub mod linear_map; pub mod range; // we define numbers as ring including the additive and multiplicative inverses // comutation on multiplication is implied (monoid). This means matrices are not // allowed, which is enforced by FromPrimitive and ToPrimitive. This is a lose -// definition, lets see if it bites us in the rear end -pub trait Number: Copy + Num + PartialOrd + PartialEq + FromPrimitive + ToPrimitive + defmt::Format {} +// definition, lets see if it bites us in the rear end +pub trait Number: + Copy + Num + PartialOrd + PartialEq + FromPrimitive + ToPrimitive + defmt::Format +{ +} -impl Number for T where T: Copy + Num + PartialOrd + PartialEq + FromPrimitive + ToPrimitive + defmt::Format {} +impl Number for T where + T: Copy + Num + PartialOrd + PartialEq + FromPrimitive + ToPrimitive + defmt::Format +{ +} diff --git a/lib-stm32/src/math/range.rs b/lib-stm32/src/math/range.rs index 95e9dffc..a1692c23 100644 --- a/lib-stm32/src/math/range.rs +++ b/lib-stm32/src/math/range.rs @@ -3,7 +3,7 @@ use super::Number; // Take input max and/or min (bounds), output value within new bounds pub struct Range where - T: Number + T: Number, { min: T, max: T, @@ -11,7 +11,7 @@ where impl Range where - T: Number + T: Number, { pub const fn new(min: T, max: T) -> Self { Range { min, max } @@ -19,7 +19,7 @@ where pub fn map_value_to_range(&self, val: T, new_range: &Range) -> T { let scale = (new_range.max - new_range.min) / (self.max - self.min); - + (val - self.min) * scale + new_range.min } @@ -31,4 +31,3 @@ where self.max } } - diff --git a/lib-stm32/src/power/battery.rs b/lib-stm32/src/power/battery.rs index 144c0407..7fda85ab 100644 --- a/lib-stm32/src/power/battery.rs +++ b/lib-stm32/src/power/battery.rs @@ -1,9 +1,10 @@ - -use crate::{filter::Filter, math::{linear_map::LinearMap, Number}}; +use crate::{ + filter::Filter, + math::{linear_map::LinearMap, Number}, +}; use super::model::lipo_model::{lipo_pct_interp, LIPO_1S_VOLTAGE_PERCENT}; - pub struct BatteryConfig { pub cell_voltage_low_warn: N, pub cell_voltage_low_crit: N, @@ -32,7 +33,11 @@ pub struct LipoModel<'a, const NUM_CELLS: usize, F: Filter> { } impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { - pub fn new(config: BatteryConfig, cell_range_maps: &'a[LinearMap; NUM_CELLS], cell_voltage_compute_mode: CellVoltageComputeMode) -> Self { + pub fn new( + config: BatteryConfig, + cell_range_maps: &'a [LinearMap; NUM_CELLS], + cell_voltage_compute_mode: CellVoltageComputeMode, + ) -> Self { Self { config, cell_range_maps, @@ -44,7 +49,6 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { } pub fn add_cell_voltage_samples(&mut self, cell_adc_voltage_samples: &[f32]) { - // place raw samples into cell_voltage buffer and use it as scratch space self.cell_voltages.copy_from_slice(cell_adc_voltage_samples); @@ -54,7 +58,11 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { } // update filters and inplace update value with filtered value - for (cv, cv_filt) in self.cell_voltages.iter_mut().zip(self.cell_votlage_filters.iter_mut()) { + for (cv, cv_filt) in self + .cell_voltages + .iter_mut() + .zip(self.cell_votlage_filters.iter_mut()) + { cv_filt.add_sample(*cv); cv_filt.update(); *cv = cv_filt.filtered_value().unwrap_or(0.0); @@ -68,7 +76,11 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { } } - for (cp, cv) in self.cell_percentages.iter_mut().zip(self.cell_voltages.into_iter()) { + for (cp, cv) in self + .cell_percentages + .iter_mut() + .zip(self.cell_voltages.into_iter()) + { *cp = lipo_pct_interp(cv, &LIPO_1S_VOLTAGE_PERCENT) as u8 } } @@ -96,7 +108,7 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { pub fn get_worst_cell_imbalance(&self) -> f32 { let mut min = f32::MAX; let mut max = f32::MIN; - + for cv in self.cell_voltages.into_iter() { min = f32::min(min, cv); max = f32::max(max, cv); @@ -110,8 +122,11 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { } pub fn battery_warn(&self) -> bool { - self.battery_cell_imbalance_warn()|| - self.get_cell_voltages().into_iter().any(|cell_voltage| *cell_voltage > self.config.cell_voltage_high_warn || *cell_voltage < self.config.cell_voltage_low_warn) + self.battery_cell_imbalance_warn() + || self.get_cell_voltages().into_iter().any(|cell_voltage| { + *cell_voltage > self.config.cell_voltage_high_warn + || *cell_voltage < self.config.cell_voltage_low_warn + }) } pub fn battery_cell_imbalance_warn(&self) -> bool { @@ -119,12 +134,18 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { } pub fn battery_crit(&self) -> bool { - self.get_worst_cell_imbalance() > self.config.cell_voltage_difference_crit || - self.get_cell_voltages().into_iter().any(|cell_voltage| *cell_voltage > self.config.cell_voltage_high_crit || *cell_voltage < self.config.cell_voltage_low_crit) + self.get_worst_cell_imbalance() > self.config.cell_voltage_difference_crit + || self.get_cell_voltages().into_iter().any(|cell_voltage| { + *cell_voltage > self.config.cell_voltage_high_crit + || *cell_voltage < self.config.cell_voltage_low_crit + }) } pub fn battery_power_off(&self) -> bool { - self.get_worst_cell_imbalance() > self.config.cell_votlage_difference_off || - self.get_cell_voltages().into_iter().any(|cell_voltage| *cell_voltage > self.config.cell_voltage_high_power_off || *cell_voltage < self.config.cell_voltage_low_power_off) + self.get_worst_cell_imbalance() > self.config.cell_votlage_difference_off + || self.get_cell_voltages().into_iter().any(|cell_voltage| { + *cell_voltage > self.config.cell_voltage_high_power_off + || *cell_voltage < self.config.cell_voltage_low_power_off + }) } -} \ No newline at end of file +} diff --git a/lib-stm32/src/power/mod.rs b/lib-stm32/src/power/mod.rs index 34a0dfa4..8b9cc7c1 100644 --- a/lib-stm32/src/power/mod.rs +++ b/lib-stm32/src/power/mod.rs @@ -1,4 +1,7 @@ -use crate::{filter::Filter, math::{range::Range, Number}}; +use crate::{ + filter::Filter, + math::{range::Range, Number}, +}; pub mod battery; pub mod model; @@ -9,7 +12,7 @@ pub struct PowerRailParameters { pub max_value_crit: T, } -pub struct PowerRail> { +pub struct PowerRail> { rail_parameters: PowerRailParameters, filter: F, sample_range: Range, @@ -17,17 +20,24 @@ pub struct PowerRail> { } impl> PowerRail { - pub fn new(rail_parameters: PowerRailParameters, filter: F, sample_range: Range, rail_range: Range) -> Self { + pub fn new( + rail_parameters: PowerRailParameters, + filter: F, + sample_range: Range, + rail_range: Range, + ) -> Self { Self { rail_parameters, filter, sample_range, - rail_range + rail_range, } } pub fn add_rail_voltage_sample(&mut self, sample: T) { - let sample_rail_domain = self.sample_range.map_value_to_range(sample, &self.rail_range); + let sample_rail_domain = self + .sample_range + .map_value_to_range(sample, &self.rail_range); self.filter.add_sample(sample_rail_domain); self.filter.update(); } @@ -61,4 +71,4 @@ impl> PowerRail { false } } -} \ No newline at end of file +} diff --git a/lib-stm32/src/power/model/lipo_model.rs b/lib-stm32/src/power/model/lipo_model.rs index 3e50225b..2fba0fc8 100644 --- a/lib-stm32/src/power/model/lipo_model.rs +++ b/lib-stm32/src/power/model/lipo_model.rs @@ -4,67 +4,70 @@ pub const LIPO_CELL_MAX_VOLTAGE: f32 = 4.2; pub fn lipo_pct_interp(voltage: f32, model: &[(f32, f32); 21]) -> f32 { if voltage <= model.first().unwrap().0 { - return model.first().unwrap().1 + return model.first().unwrap().1; } if voltage >= model.last().unwrap().0 { - return model.last().unwrap().1 + return model.last().unwrap().1; } for i in 0..20 { - if model[i].0 <= voltage && voltage < model[i+1].0 { - return LinearMap::::map_ranges_bounded(voltage, Range::new(model[i].0, model[i+1].0), Range::new(model[i].1, model[i+1].1)) + if model[i].0 <= voltage && voltage < model[i + 1].0 { + return LinearMap::::map_ranges_bounded( + voltage, + Range::new(model[i].0, model[i + 1].0), + Range::new(model[i].1, model[i + 1].1), + ); } } model.first().unwrap().1 } -pub const LIPO_1S_VOLTAGE_PERCENT: [(f32, f32); 21] = [ - (3.27, 0.0), - (3.61, 5.0), - (3.69, 10.0), - (3.71, 15.0), - (3.73, 20.0), - (3.75, 25.0), - (3.77, 30.0), - (3.79, 35.0), - (3.80, 40.0), - (3.82, 45.0), - (3.84, 50.0), - (3.85, 55.0), - (3.87, 60.0), - (3.91, 65.0), - (3.95, 70.0), - (3.98, 75.0), - (4.02, 80.0), - (4.08, 85.0), - (4.11, 90.0), - (4.15, 95.0), +pub const LIPO_1S_VOLTAGE_PERCENT: [(f32, f32); 21] = [ + (3.27, 0.0), + (3.61, 5.0), + (3.69, 10.0), + (3.71, 15.0), + (3.73, 20.0), + (3.75, 25.0), + (3.77, 30.0), + (3.79, 35.0), + (3.80, 40.0), + (3.82, 45.0), + (3.84, 50.0), + (3.85, 55.0), + (3.87, 60.0), + (3.91, 65.0), + (3.95, 70.0), + (3.98, 75.0), + (4.02, 80.0), + (4.08, 85.0), + (4.11, 90.0), + (4.15, 95.0), (4.20, 100.0), ]; - pub const LIPO_6S_VOLTAGE_PERCENT: [(f32, f32); 21] = [ - (19.64, 0.0), - (21.65, 5.0), - (22.12, 10.0), - (22.24, 15.0), - (22.36, 20.0), - (22.48, 25.0), - (22.60, 30.0), - (22.72, 35.0), - (22.77, 40.0), - (22.89, 45.0), - (23.01, 50.0), - (23.13, 55.0), - (23.25, 60.0), - (23.48, 65.0), - (23.72, 70.0), - (23.90, 75.0), - (24.14, 80.0), - (24.49, 85.0), - (24.67, 90.0), - (24.90, 95.0), + (19.64, 0.0), + (21.65, 5.0), + (22.12, 10.0), + (22.24, 15.0), + (22.36, 20.0), + (22.48, 25.0), + (22.60, 30.0), + (22.72, 35.0), + (22.77, 40.0), + (22.89, 45.0), + (23.01, 50.0), + (23.13, 55.0), + (23.25, 60.0), + (23.48, 65.0), + (23.72, 70.0), + (23.90, 75.0), + (24.14, 80.0), + (24.49, 85.0), + (24.67, 90.0), + (24.90, 95.0), (25.20, 100.0), -]; \ No newline at end of file +]; diff --git a/lib-stm32/src/power/model/mod.rs b/lib-stm32/src/power/model/mod.rs index 1f19169a..d01cbcca 100644 --- a/lib-stm32/src/power/model/mod.rs +++ b/lib-stm32/src/power/model/mod.rs @@ -1,3 +1,3 @@ pub mod lipo_model; -// pub fn battery_voltage_to_percent() \ No newline at end of file +// pub fn battery_voltage_to_percent() diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index 01bf6380..ba50a1c4 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -1,5 +1,8 @@ use core::{ - cell::{SyncUnsafeCell, UnsafeCell}, future::poll_fn, sync::atomic::{AtomicBool, AtomicUsize, Ordering}, task::{Poll, Waker} + cell::{SyncUnsafeCell, UnsafeCell}, + future::poll_fn, + sync::atomic::{AtomicBool, AtomicUsize, Ordering}, + task::{Poll, Waker}, }; use critical_section; @@ -24,7 +27,7 @@ impl Buffer { pub const EMPTY: Buffer = Buffer { data: [0_u8; LENGTH], - len: 0 + len: 0, }; } @@ -95,7 +98,6 @@ pub struct Queue { unsafe impl Send for Queue {} unsafe impl Sync for Queue {} - impl Queue { pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { // we must at least double buffer in mixed priority execution @@ -182,7 +184,10 @@ impl Queue { if self.read_in_progress.load(Ordering::SeqCst) { self.read_in_progress.store(false, Ordering::SeqCst); - self.read_index.store((self.read_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.read_index.store( + (self.read_index.load(Ordering::SeqCst) + 1) % DEPTH, + Ordering::SeqCst, + ); // NOTE: this was an atomic fetch_add but that isn't supported on // thumbv6m @@ -211,7 +216,8 @@ impl Queue { * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but * not both at once. */ - let buf = unsafe { &mut *self.buffers[self.write_index.load(Ordering::SeqCst)].get() }; + let buf = + unsafe { &mut *self.buffers[self.write_index.load(Ordering::SeqCst)].get() }; /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file * (where a user can specify the link section) and so the compiler knows the type and satisfied * defined behavior constraints w.r.t data alignment and init values, and therefore referencing @@ -246,9 +252,9 @@ impl Queue { self.write_in_progress.store(true, Ordering::SeqCst); /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. - * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but - * not both at once. - */ + * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but + * not both at once. + */ // we will return an available write buffer // if the queue is currently full, we need to evict the tail entry @@ -270,20 +276,19 @@ impl Queue { // write back decremented size, this guards the dequeue from using the // entry and seeing it as valid. This is valid because we know any pending dequeue // must be on another buffer beacuse the queue is full, and the assert in the constructor - // requres we are *at least* double buffered, so any dequeue in a lower prio thread is + // requres we are *at least* double buffered, so any dequeue in a lower prio thread is // pointing to the other buffer meaning we can just purge the current one self.size.store(cur_size - 1, Ordering::SeqCst); // the memory in the backing buffer intact but it's now available for writing } - let buf = unsafe { &mut *self.buffers[write_index].get() }; /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file - * (where a user can specify the link section) and so the compiler knows the type and satisfied - * defined behavior constraints w.r.t data alignment and init values, and therefore referencing - * the buffer means the internal data is valid. - */ + * (where a user can specify the link section) and so the compiler knows the type and satisfied + * defined behavior constraints w.r.t data alignment and init values, and therefore referencing + * the buffer means the internal data is valid. + */ let data = &mut buf.data; // TODO CHCEK: https://doc.rust-lang.org/std/mem/union.MaybeUninit.html#method.write-1 this should overwrite the value and @@ -333,7 +338,10 @@ impl Queue { if self.write_in_progress.load(Ordering::SeqCst) { self.write_in_progress.store(false, Ordering::SeqCst); - self.write_index.store((self.write_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.write_index.store( + (self.write_index.load(Ordering::SeqCst) + 1) % DEPTH, + Ordering::SeqCst, + ); // NOTE: this was an atomic fetch_add but that isn't supported on // thumbv6m diff --git a/lib-stm32/src/time/mod.rs b/lib-stm32/src/time/mod.rs index 63ae87cb..d2de1fb7 100644 --- a/lib-stm32/src/time/mod.rs +++ b/lib-stm32/src/time/mod.rs @@ -34,14 +34,12 @@ pub struct Limiter { impl Limiter { pub const fn new() -> Self { - Limiter { - prev_val: None - } + Limiter { prev_val: None } } pub const fn new_with_value(val: T) -> Self { Limiter { - prev_val: Some(val) + prev_val: Some(val), } } @@ -56,4 +54,4 @@ impl Limiter { self.prev_val = Some(val); res } -} \ No newline at end of file +} diff --git a/lib-stm32/src/uart/mod.rs b/lib-stm32/src/uart/mod.rs index 417c539d..53c4e722 100644 --- a/lib-stm32/src/uart/mod.rs +++ b/lib-stm32/src/uart/mod.rs @@ -1,2 +1,2 @@ pub mod queue; -// pub mod tasks; \ No newline at end of file +// pub mod tasks; diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 7768f6dc..80dff948 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -1,10 +1,13 @@ #![warn(async_fn_in_trait)] -use core::{future::Future, sync::atomic::{AtomicBool, Ordering}}; +use core::{ + future::Future, + sync::atomic::{AtomicBool, Ordering}, +}; use embassy_stm32::{ mode::Async, - usart::{self, UartRx, UartTx} + usart::{self, UartRx, UartTx}, }; use embassy_futures::select::{select, Either}; @@ -15,33 +18,28 @@ use embassy_sync::{ }; use embassy_time::Timer; -use crate::queue::{ - self, - DequeueRef, - Error, - Queue -}; +use crate::queue::{self, DequeueRef, Error, Queue}; #[macro_export] macro_rules! static_idle_buffered_uart_nl { ($name:ident, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $debug:expr) => { $crate::paste::paste! { - static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = + static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = [const { core::cell::SyncUnsafeCell::new($crate::queue::Buffer::<$rx_buffer_size>::new()) }; $rx_buffer_depth]; - static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = + static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = [const { core::cell::SyncUnsafeCell::new($crate::queue::Buffer::<$tx_buffer_size>::new()) }; $tx_buffer_depth]; - static [<$name:upper _IDLE_BUFFERED_UART>]: $crate::uart::queue::IdleBufferedUart<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug> = + static [<$name:upper _IDLE_BUFFERED_UART>]: $crate::uart::queue::IdleBufferedUart<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug> = $crate::uart::queue::IdleBufferedUart::new( $crate::queue::Queue::new(&[<$name _RX_BUFFER>]), $crate::queue::Queue::new(&[<$name _TX_BUFFER>]) ); static [<$name:upper _READ_TASK_STORAGE>]: embassy_executor::raw::TaskStorage< - $crate::uart::queue::IdleBufferedUartReadFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = + $crate::uart::queue::IdleBufferedUartReadFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = embassy_executor::raw::TaskStorage::new(); static [<$name:upper _WRITE_TASK_STORAGE>]: embassy_executor::raw::TaskStorage< - $crate::uart::queue::IdleBufferedUartWriteFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = + $crate::uart::queue::IdleBufferedUartWriteFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = embassy_executor::raw::TaskStorage::new(); } } @@ -52,23 +50,23 @@ macro_rules! static_idle_buffered_uart { ($name:ident, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $debug:expr, $(#[$m:meta])*) => { $crate::paste::paste! { $(#[$m])* - static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = + static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = [const { core::cell::SyncUnsafeCell::new($crate::queue::Buffer::<$rx_buffer_size>::new()) }; $rx_buffer_depth]; $(#[$m])* - static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = + static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = [const { core::cell::SyncUnsafeCell::new($crate::queue::Buffer::<$tx_buffer_size>::new()) }; $tx_buffer_depth]; - static [<$name:upper _IDLE_BUFFERED_UART>]: $crate::uart::queue::IdleBufferedUart<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug> = + static [<$name:upper _IDLE_BUFFERED_UART>]: $crate::uart::queue::IdleBufferedUart<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug> = $crate::uart::queue::IdleBufferedUart::new( $crate::queue::Queue::new(&[<$name _RX_BUFFER>]), $crate::queue::Queue::new(&[<$name _TX_BUFFER>]) ); static [<$name:upper _READ_TASK_STORAGE>]: embassy_executor::raw::TaskStorage< - $crate::uart::queue::IdleBufferedUartReadFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = + $crate::uart::queue::IdleBufferedUartReadFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = embassy_executor::raw::TaskStorage::new(); static [<$name:upper _WRITE_TASK_STORAGE>]: embassy_executor::raw::TaskStorage< - $crate::uart::queue::IdleBufferedUartWriteFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = + $crate::uart::queue::IdleBufferedUartWriteFuture<$rx_buffer_size, $rx_buffer_depth, $tx_buffer_size, $tx_buffer_depth, $debug>> = embassy_executor::raw::TaskStorage::new(); } } @@ -104,12 +102,17 @@ macro_rules! idle_buffered_uart_spawn_tasks { } type UartQueueSyncCommandPubSub = PubSubChannel; -type UartQueueSyncCommandSub = Subscriber<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 2, 1>; -type UartQueueSyncCommandPub = Publisher<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 2, 1>; - -type UartQueueSyncResponsePubSub = PubSubChannel; -type UartQueueSyncResponseSub = Subscriber<'static, CriticalSectionRawMutex, UartTaskResponse, 2, 1, 2>; -type UartQueueSyncResponsePub = Publisher<'static, CriticalSectionRawMutex, UartTaskResponse, 2, 1, 2>; +type UartQueueSyncCommandSub = + Subscriber<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 2, 1>; +type UartQueueSyncCommandPub = + Publisher<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 2, 1>; + +type UartQueueSyncResponsePubSub = + PubSubChannel; +type UartQueueSyncResponseSub = + Subscriber<'static, CriticalSectionRawMutex, UartTaskResponse, 2, 1, 2>; +type UartQueueSyncResponsePub = + Publisher<'static, CriticalSectionRawMutex, UartTaskResponse, 2, 1, 2>; pub type IdleBufferedUartReadFuture< const RLEN: usize, @@ -127,23 +130,15 @@ pub type IdleBufferedUartWriteFuture< const DEBUG: bool, > = impl Future; -pub type ReadTaskFuture< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, -> = impl Future; +pub type ReadTaskFuture = impl Future; -pub type WriteTaskFuture< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, -> = impl Future; +pub type WriteTaskFuture = impl Future; #[derive(Clone, Copy, PartialEq, Debug)] pub enum UartTaskCommand { Pause, UpdateConfig(usart::Config), - Unpause + Unpause, } #[derive(Clone, Copy, PartialEq, Debug)] @@ -170,22 +165,20 @@ pub struct IdleBufferedUart< uart_config_command_pubsub: UartQueueSyncCommandPubSub, uart_config_response_pubsub: UartQueueSyncResponsePubSub, uart_config_command_publisher: Mutex>, - uart_config_response_subscriber: Mutex>, + uart_config_response_subscriber: + Mutex>, } -impl < - const RLEN: usize, - const RDEPTH: usize, - const WLEN: usize, - const WDEPTH: usize, - const DEBUG: bool, - > IdleBufferedUart +impl< + const RLEN: usize, + const RDEPTH: usize, + const WLEN: usize, + const WDEPTH: usize, + const DEBUG: bool, + > IdleBufferedUart { - pub const fn new( - read_queue: Queue, - write_queue: Queue, - ) -> Self { - IdleBufferedUart { + pub const fn new(read_queue: Queue, write_queue: Queue) -> Self { + IdleBufferedUart { uart_read_queue: UartReadQueue::new(read_queue), uart_write_queue: UartWriteQueue::new(write_queue), uart_config_initialized: AtomicBool::new(false), @@ -206,10 +199,17 @@ impl < rx: UartRx<'static, Async>, ) -> IdleBufferedUartReadFuture { async move { - self.uart_read_queue.read_task(rx, - self.uart_config_command_pubsub.subscriber().expect("uart read task command sub failed"), - self.uart_config_response_pubsub.publisher().expect("uart read task reponse pub failed") - ).await + self.uart_read_queue + .read_task( + rx, + self.uart_config_command_pubsub + .subscriber() + .expect("uart read task command sub failed"), + self.uart_config_response_pubsub + .publisher() + .expect("uart read task reponse pub failed"), + ) + .await } } @@ -222,20 +222,31 @@ impl < tx: UartTx<'static, Async>, ) -> IdleBufferedUartWriteFuture { async move { - self.uart_write_queue.write_task(tx, - self.uart_config_command_pubsub.subscriber().expect("uart write task command sub failed"), - self.uart_config_response_pubsub.publisher().expect("uart write task reponse pub failed") - ).await + self.uart_write_queue + .write_task( + tx, + self.uart_config_command_pubsub + .subscriber() + .expect("uart write task command sub failed"), + self.uart_config_response_pubsub + .publisher() + .expect("uart write task reponse pub failed"), + ) + .await } } pub fn init(&'static self) { if let Ok(mut command_pub) = self.uart_config_command_publisher.try_lock() { - command_pub.replace(self.uart_config_command_pubsub.publisher().expect("uart config command publisher already consumed. Did the init guard fail?")); + command_pub.replace(self.uart_config_command_pubsub.publisher().expect( + "uart config command publisher already consumed. Did the init guard fail?", + )); } if let Ok(mut response_sub) = self.uart_config_response_subscriber.try_lock() { - response_sub.replace(self.uart_config_response_pubsub.subscriber().expect("uart config response subscriber already consumed. Did the init guard fail?")); + response_sub.replace(self.uart_config_response_pubsub.subscriber().expect( + "uart config response subscriber already consumed. Did the init guard fail?", + )); } self.uart_config_initialized.store(true, Ordering::SeqCst); @@ -243,18 +254,25 @@ impl < pub async fn update_uart_config(&self, config: usart::Config) -> Result<(), ()> { #[cfg(target_has_atomic)] - if let Err(_) = self.uart_config_update_in_progress.compare_exchange(false, true, Ordering::Acquire, Ordering::Relaxed) { + if let Err(_) = self.uart_config_update_in_progress.compare_exchange( + false, + true, + Ordering::Acquire, + Ordering::Relaxed, + ) { return Err(()); } - #[cfg(not(target_has_atomic))] { + #[cfg(not(target_has_atomic))] + { let mut early_ret = false; critical_section::with(|_| { let update_in_progress = self.uart_config_update_in_progress.load(Ordering::SeqCst); if update_in_progress { early_ret = true; } else { - self.uart_config_update_in_progress.store(true, Ordering::SeqCst); + self.uart_config_update_in_progress + .store(true, Ordering::SeqCst); } }); @@ -270,10 +288,14 @@ impl < // command/response mutex scope { let mut command_pub_mutex_guard = self.uart_config_command_publisher.lock().await; - let command_pub = command_pub_mutex_guard.as_mut().expect("command pub uninitialized"); + let command_pub = command_pub_mutex_guard + .as_mut() + .expect("command pub uninitialized"); let mut response_sub_mutex_guard = self.uart_config_response_subscriber.lock().await; - let response_sub = response_sub_mutex_guard.as_mut().expect("response sub uninitialized"); + let response_sub = response_sub_mutex_guard + .as_mut() + .expect("response sub uninitialized"); // tell read and write tasks to pause defmt::trace!("instructing uart tasks for pause for config update"); @@ -287,42 +309,47 @@ impl < UartTaskResponse::ReadTaskAcceptingConfig => { rx_task_paused = true; defmt::trace!("uart read task paused, accepting config update"); - }, + } UartTaskResponse::WriteTaskPaused => { tx_task_paused = true; defmt::trace!("uart write task paused for config update"); - }, + } _ => { - defmt::warn!("received spurious value while waiting for uart tasks to pause"); - }, + defmt::warn!( + "received spurious value while waiting for uart tasks to pause" + ); + } } } defmt::trace!("uart tasks ready for config update"); // send new config - command_pub.publish(UartTaskCommand::UpdateConfig(config)).await; + command_pub + .publish(UartTaskCommand::UpdateConfig(config)) + .await; // get confirmation the read queue updated the config - 'config_update_loop: - loop { + 'config_update_loop: loop { match response_sub.next_message_pure().await { UartTaskResponse::UpdateSuccessful => { config_update_success = true; break 'config_update_loop; - }, + } UartTaskResponse::UpdateFailed => { break 'config_update_loop; - }, + } _ => { - defmt::warn!("received spurious value while waiting for read task to update config"); - }, + defmt::warn!( + "received spurious value while waiting for read task to update config" + ); + } } } - + // tell tasks to unpause command_pub.publish(UartTaskCommand::Unpause).await; - + // confirm tasks are running let mut rx_task_running = false; let mut tx_task_running = false; @@ -331,33 +358,46 @@ impl < UartTaskResponse::ReadTaskRunning => { rx_task_running = true; defmt::trace!("uart read task resumed"); - }, + } UartTaskResponse::WriteTaskRunning => { tx_task_running = true; defmt::trace!("uart write task resumed"); - }, + } _ => { - defmt::warn!("received spurious value while waiting for uart tasks to resume"); - }, + defmt::warn!( + "received spurious value while waiting for uart tasks to resume" + ); + } } } - // pub and sub mutex guards dropped here + // pub and sub mutex guards dropped here } #[cfg(target_has_atomic)] - if let Err(_) = self.uart_config_update_in_progress.compare_exchange(true, false, Ordering::SeqCst, Ordering::Acquire) { - defmt::error!("uart config update state became inchoerant. This should not be possible."); + if let Err(_) = self.uart_config_update_in_progress.compare_exchange( + true, + false, + Ordering::SeqCst, + Ordering::Acquire, + ) { + defmt::error!( + "uart config update state became inchoerant. This should not be possible." + ); } - #[cfg(not(target_has_atomic))] { + #[cfg(not(target_has_atomic))] + { critical_section::with(|_| { let update_in_progress = self.uart_config_update_in_progress.load(Ordering::SeqCst); if !update_in_progress { - defmt::error!("uart config update state became inchoerant. This should not be possible."); + defmt::error!( + "uart config update state became inchoerant. This should not be possible." + ); } - self.uart_config_update_in_progress.store(false, Ordering::SeqCst); + self.uart_config_update_in_progress + .store(false, Ordering::SeqCst); }); } @@ -387,11 +427,11 @@ impl < // let mut ret_val: Result<(), ()> = Err(()); // { // let mut success_subscriber = self.uart_config_signal_subscriber.lock().await; - + // // wait for tasks to indicate success // loop { // let success_result = success_subscriber.as_mut().unwrap().next_message().await; - // match success_result { + // match success_result { // WaitResult::Lagged(amnt) => { // defmt::debug!("UartQueue - lagged {} waiting for status response from config update", amnt); // } @@ -415,25 +455,15 @@ impl < } } -pub struct UartReadQueue< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, -> { +pub struct UartReadQueue { queue_rx: Queue, } -impl< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, - > UartReadQueue +impl + UartReadQueue { - pub const fn new( - queue: Queue) -> Self { - Self { - queue_rx: queue, - } + pub const fn new(queue: Queue) -> Self { + Self { queue_rx: queue } } fn read_task( @@ -460,7 +490,7 @@ impl< // // release the uart hw mutex lock by dropping the MutexGuard // drop(rw_tasks_config_lock.take().unwrap()); - // match uart_config_signal_subscriber.next_message().await { + // match uart_config_signal_subscriber.next_message().await { // WaitResult::Lagged(amnt) => { // defmt::debug!("UartReadQueue - lagged {} processing config signal", amnt) // } @@ -474,8 +504,7 @@ impl< // } // } // } - // } - + // } // get enqueue ref to pass to the DMA layer // let mut buf = self.queue_rx.enqueue().await.unwrap(); @@ -484,7 +513,12 @@ impl< // } let mut buf = self.queue_rx.try_enqueue_override().expect("multiple threads concurrently attempted to bind an internal queue buffer to dma"); - match select(rx.read_until_idle(buf.data()), uart_config_command_subscriber.next_message()).await { + match select( + rx.read_until_idle(buf.data()), + uart_config_command_subscriber.next_message(), + ) + .await + { Either::First(len) => { if let Ok(len) = len { if len == 0 { @@ -498,7 +532,7 @@ impl< // defmt::warn!("{}", len); buf.cancel(); } - }, + } Either::Second(config_signal_result) => { defmt::trace!("UartReadQueue - read to idle cancelled to update config."); // clear the buffer record keeping, the transaction may have been interrupted @@ -511,30 +545,48 @@ impl< WaitResult::Message(task_command) => { if task_command == UartTaskCommand::Pause { // send config ready - uart_config_response_publisher.publish(UartTaskResponse::ReadTaskAcceptingConfig).await; + uart_config_response_publisher + .publish(UartTaskResponse::ReadTaskAcceptingConfig) + .await; // wait for issuance of new config - if let UartTaskCommand::UpdateConfig(config) = uart_config_command_subscriber.next_message_pure().await { + if let UartTaskCommand::UpdateConfig(config) = + uart_config_command_subscriber.next_message_pure().await + { if let Err(_cfg_err) = rx.set_config(&config) { - defmt::error!("uart read task config update failed {:?}, {:?}", _cfg_err as u8, usart::ConfigError::DataParityNotSupported as u8); - uart_config_response_publisher.publish(UartTaskResponse::UpdateFailed).await; + defmt::error!( + "uart read task config update failed {:?}, {:?}", + _cfg_err as u8, + usart::ConfigError::DataParityNotSupported as u8 + ); + uart_config_response_publisher + .publish(UartTaskResponse::UpdateFailed) + .await; } else { defmt::trace!("uart read task updated config"); - uart_config_response_publisher.publish(UartTaskResponse::UpdateSuccessful).await; + uart_config_response_publisher + .publish(UartTaskResponse::UpdateSuccessful) + .await; } } else { defmt::error!("invalid config update command"); }; // wait for command to resume - if let UartTaskCommand::Unpause = uart_config_command_subscriber.next_message_pure().await { - defmt::trace!("uart read task resuming after config update"); + if let UartTaskCommand::Unpause = + uart_config_command_subscriber.next_message_pure().await + { + defmt::trace!( + "uart read task resuming after config update" + ); } else { defmt::error!("invalid config resume command"); } // report resuming - uart_config_response_publisher.publish(UartTaskResponse::ReadTaskRunning).await; + uart_config_response_publisher + .publish(UartTaskResponse::ReadTaskRunning) + .await; } else { defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); } @@ -546,8 +598,6 @@ impl< } } - - pub fn can_dequque(&self) -> bool { self.queue_rx.can_dequeue() } @@ -562,26 +612,15 @@ impl< } } -pub struct UartWriteQueue< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, -> { +pub struct UartWriteQueue { queue_tx: Queue, } -impl< - const LENGTH: usize, - const DEPTH: usize, - const DEBUG: bool, - > UartWriteQueue +impl + UartWriteQueue { - pub const fn new( - queue: Queue, - ) -> Self { - Self { - queue_tx: queue, - } + pub const fn new(queue: Queue) -> Self { + Self { queue_tx: queue } } fn write_task( @@ -595,7 +634,12 @@ impl< // the tx task primarily blocks on queue_tx.queue(), e.g. waiting for other async tasks // to enqueue data. Use a select to break waiting if another task signals there's a UART // config update. They probably want the update before the next data is enqueued - match select(self.queue_tx.dequeue(), uart_config_command_subscriber.next_message()).await { + match select( + self.queue_tx.dequeue(), + uart_config_command_subscriber.next_message(), + ) + .await + { // we are dequeing data Either::First(dq_res) => { if DEBUG { @@ -632,24 +676,34 @@ impl< } WaitResult::Message(task_command) => { if task_command == UartTaskCommand::Pause { - uart_config_response_publisher.publish(UartTaskResponse::WriteTaskPaused).await; + uart_config_response_publisher + .publish(UartTaskResponse::WriteTaskPaused) + .await; - if let UartTaskCommand::UpdateConfig(_) = uart_config_command_subscriber.next_message_pure().await { + if let UartTaskCommand::UpdateConfig(_) = + uart_config_command_subscriber.next_message_pure().await + { defmt::trace!("uart write task received config, deferring to read for update"); } else { defmt::error!("uart write task received invalid command expecting config update"); } // read task should perform the update, expect unpause command soon - if let UartTaskCommand::Unpause = uart_config_command_subscriber.next_message_pure().await { + if let UartTaskCommand::Unpause = + uart_config_command_subscriber.next_message_pure().await + { defmt::trace!("uart write task received unpause"); } else { defmt::error!("uart write task received invalid command expecting unpause"); } - uart_config_response_publisher.publish(UartTaskResponse::WriteTaskRunning).await; + uart_config_response_publisher + .publish(UartTaskResponse::WriteTaskRunning) + .await; } else { - defmt::warn!("uart write task received spurious command during write."); + defmt::warn!( + "uart write task received spurious command during write." + ); } } } @@ -675,29 +729,29 @@ impl< } pub trait Reader { - fn read RET>(&self, fn_read: FN) -> impl core::future::Future>; + fn read RET>( + &self, + fn_read: FN, + ) -> impl core::future::Future>; } pub trait Writer { - fn write usize>(&self, fn_write: FN) -> impl core::future::Future>; + fn write usize>( + &self, + fn_write: FN, + ) -> impl core::future::Future>; } -impl< - const LEN: usize, - const DEPTH: usize, - const DEBUG: bool, - > Reader for UartReadQueue +impl Reader + for UartReadQueue { async fn read RET>(&self, fn_read: FN) -> Result { Ok(self.dequeue(|buf| fn_read(buf)).await) } } -impl< - const LEN: usize, - const DEPTH: usize, - const DEBUG: bool, - > Writer for UartWriteQueue +impl Writer + for UartWriteQueue { async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { self.enqueue(|buf| fn_write(buf)).or(Err(())) diff --git a/lib-stm32/src/units.rs b/lib-stm32/src/units.rs index ffb89725..abe0b1df 100644 --- a/lib-stm32/src/units.rs +++ b/lib-stm32/src/units.rs @@ -1,3 +1,2 @@ - pub const MV_PER_V: u16 = 1000; -pub const V_PER_MV: f32 = 1.0 / 1000.0; \ No newline at end of file +pub const V_PER_MV: f32 = 1.0 / 1000.0; diff --git a/lib-stm32/tests/linear-map.rs b/lib-stm32/tests/linear-map.rs index 5a537cdf..6ce23d88 100644 --- a/lib-stm32/tests/linear-map.rs +++ b/lib-stm32/tests/linear-map.rs @@ -1,13 +1,12 @@ #![no_std] #![no_main] -use panic_probe as _; use defmt_rtt as _; +use panic_probe as _; #[defmt_test::tests] -mod tests{ +mod tests { use ateam_lib_stm32::math::linear::LinearMap; - #[test] fn small_to_large() { diff --git a/power-board/src/bin/hwtest-bringup/main.rs b/power-board/src/bin/hwtest-bringup/main.rs index 4ca78e1d..c97888a8 100644 --- a/power-board/src/bin/hwtest-bringup/main.rs +++ b/power-board/src/bin/hwtest-bringup/main.rs @@ -5,22 +5,35 @@ use defmt::*; use embassy_executor::Spawner; -use embassy_stm32::{adc::{Adc, AdcChannel, SampleTime}, - gpio::{Input, Level, Output, OutputOpenDrain, Pull, Speed, OutputType}, - timer::{simple_pwm::{SimplePwm, PwmPin}, Channel}, - time::hz}; +use embassy_stm32::{ + adc::{Adc, AdcChannel, SampleTime}, + gpio::{Input, Level, Output, OutputOpenDrain, OutputType, Pull, Speed}, + time::hz, + timer::{ + simple_pwm::{PwmPin, SimplePwm}, + Channel, + }, +}; use embassy_time::Timer; use {defmt_rtt as _, panic_probe as _}; -use ateam_lib_stm32::{drivers::{led::apa102::Apa102, - audio::buzzer::Buzzer}, audio::{note::Beat, tone_player::TonePlayer}}; -use smart_leds::colors::{BLUE, WHITE, BLACK}; +use ateam_lib_stm32::{ + audio::{note::Beat, tone_player::TonePlayer}, + drivers::{audio::buzzer::Buzzer, led::apa102::Apa102}, +}; +use smart_leds::colors::{BLACK, BLUE, WHITE}; static mut DOTSTAR_SPI_BUFFER_CELL: [u8; 16] = [0; 16]; pub const TEST_SONG: [Beat; 2] = [ - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 587, duration: 250_000 }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 587, + duration: 250_000, + }, ]; #[embassy_executor::main] @@ -57,10 +70,11 @@ async fn main(_spawner: Spawner) { let mut vrefint_channel = adc.enable_vrefint().degrade_adc(); // Use the pin constant directly - // Get the pins from the schematic + // Get the pins from the schematic let dotstar_spi_buf: &'static mut [u8; 16] = unsafe { &mut DOTSTAR_SPI_BUFFER_CELL }; // Dotstar SPI, SCK, MOSI, and TX_DMA - let mut dotstars = Apa102::<2>::new_from_pins(p.SPI1, p.PB3, p.PB5, p.DMA1_CH2, dotstar_spi_buf.into()); + let mut dotstars = + Apa102::<2>::new_from_pins(p.SPI1, p.PB3, p.PB5, p.DMA1_CH2, dotstar_spi_buf.into()); dotstars.set_drv_str_all(32); let mut adc_buf: [u16; 7] = [0; 7]; @@ -68,8 +82,16 @@ async fn main(_spawner: Spawner) { // p.PC6 - Buzzer io pin let ch1 = PwmPin::new_ch1(p.PC6, OutputType::PushPull); // p.TIM3 - Buzzer timer - let pwm = SimplePwm::new(p.TIM3, Some(ch1), None, None, None, hz(1), Default::default()); - + let pwm = SimplePwm::new( + p.TIM3, + Some(ch1), + None, + None, + None, + hz(1), + Default::default(), + ); + let audio_driver = Buzzer::new(pwm, Channel::Ch1); let mut tone_player = TonePlayer::new(audio_driver); @@ -84,21 +106,23 @@ async fn main(_spawner: Spawner) { (&mut cell4_adc_pin, SampleTime::CYCLES160_5), (&mut cell5_adc_pin, SampleTime::CYCLES160_5), (&mut vrefint_channel, SampleTime::CYCLES160_5), - - ].into_iter(), - &mut adc_buf - ).await; + ] + .into_iter(), + &mut adc_buf, + ) + .await; let vrefint_sample = adc_buf[6]; let convert_to_millivolts = |sample: u16| { // From https://www.st.com/resource/en/datasheet/stm32g031g8.pdf // 6.3.3 Embedded internal reference voltage const VREFINT_MV: u32 = 1212; // mV - + (u32::from(sample) * VREFINT_MV / u32::from(vrefint_sample)) as u16 }; - info!("cell0: {}, cell1: {}, cell2: {}, cell3: {}, cell4: {}, cell5: {}", + info!( + "cell0: {}, cell1: {}, cell2: {}, cell3: {}, cell4: {}, cell5: {}", convert_to_millivolts(adc_buf[0]), convert_to_millivolts(adc_buf[1]), convert_to_millivolts(adc_buf[2]), @@ -118,7 +142,7 @@ async fn main(_spawner: Spawner) { // en_3v3.set_high(); // en_5v0.set_high(); Timer::after_millis(1000).await; - + let _ = tone_player.load_song(&TEST_SONG); // tone_player.play_song().await; diff --git a/power-board/src/bin/power/main.rs b/power-board/src/bin/power/main.rs index 9f8e19f6..1af37e69 100644 --- a/power-board/src/bin/power/main.rs +++ b/power-board/src/bin/power/main.rs @@ -8,12 +8,12 @@ use ateam_lib_stm32::audio::songs::SongId; use ateam_lib_stm32::audio::AudioCommand; use ateam_power_board::pins::{AudioPubSub, TelemetryPubSub}; use ateam_power_board::power_state::SharedPowerState; -use ateam_power_board::{create_power_task, create_coms_task, create_audio_task}; +use ateam_power_board::{create_audio_task, create_coms_task, create_power_task}; use defmt::*; use embassy_executor::{InterruptExecutor, Spawner}; +use embassy_stm32::gpio::{Input, Level, Output, OutputOpenDrain, Pull, Speed}; use embassy_stm32::interrupt; use embassy_stm32::interrupt::{InterruptExt, Priority}; -use embassy_stm32::gpio::{Input, Level, Output, OutputOpenDrain, Pull, Speed}; use embassy_sync::pubsub::PubSubChannel; use embassy_time::{Duration, Instant, Ticker, Timer}; use {defmt_rtt as _, panic_probe as _}; @@ -31,7 +31,6 @@ unsafe fn USART2() { UART_QUEUE_EXECUTOR.on_interrupt(); } - #[embassy_executor::main] async fn main(spawner: Spawner) { let p = embassy_stm32::init(Default::default()); @@ -67,48 +66,67 @@ async fn main(spawner: Spawner) { let power_audio_subscriber = AUDIO_PUBSUB.subscriber().unwrap(); // start power task - create_power_task!(spawner, shared_power_state, power_telemetry_publisher, power_audio_publisher, p); + create_power_task!( + spawner, + shared_power_state, + power_telemetry_publisher, + power_audio_publisher, + p + ); // start coms task - create_coms_task!(spawner, uart_queue_spawner, shared_power_state, coms_telemetry_subscriber, coms_audio_publisher, p); + create_coms_task!( + spawner, + uart_queue_spawner, + shared_power_state, + coms_telemetry_subscriber, + coms_audio_publisher, + p + ); // start audio task create_audio_task!(spawner, power_audio_subscriber, p); - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::PowerOn)).await; + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::PowerOn)) + .await; // TODO: start LED animation task - let mut main_loop_ticker = Ticker::every(Duration::from_millis(10)); loop { // read pwr button // shortest possible interrupt from pwr btn controller is 32ms if pwr_btn.get_level() == Level::Low || SHARED_POWER_STATE.get_shutdown_requested().await { - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::ShutdownRequested)).await; + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::ShutdownRequested)) + .await; Timer::after_millis(250).await; - shutdown_ind.set_low(); // indicate shutdown request + shutdown_ind.set_low(); // indicate shutdown request let shutdown_requested_time = Instant::now(); - SHARED_POWER_STATE.set_shutdown_requested(true).await; // update power board state + SHARED_POWER_STATE.set_shutdown_requested(true).await; // update power board state loop { let cur_robot_state = shared_power_state.get_state().await; - + if !SHARED_POWER_STATE.get_shutdown_requested().await { defmt::info!("MAIN TASK: Shutdown cancelled"); shutdown_ind.set_high(); - break + break; } - if SHARED_POWER_STATE.get_shutdown_ready().await - || Instant::now() > shutdown_requested_time + Duration::from_secs(30) - || !cur_robot_state.coms_established { - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::PowerOff)).await; + if SHARED_POWER_STATE.get_shutdown_ready().await + || Instant::now() > shutdown_requested_time + Duration::from_secs(30) + || !cur_robot_state.coms_established + { + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::PowerOff)) + .await; // Wait long enough for the song to play :) Timer::after_millis(500).await; defmt::info!("MAIN TASK: Shutdown acknowledged, turning off power"); Timer::after_millis(500).await; sequence_power_off(&mut en_3v3, &mut en_5v0, &mut en_12v0).await; kill_sig.set_low(); - break + break; } Timer::after_millis(10).await; @@ -119,7 +137,11 @@ async fn main(spawner: Spawner) { } } -async fn sequence_power_on(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'static>, en_12v0: &mut Output<'static>) { +async fn sequence_power_on( + en_3v3: &mut Output<'static>, + en_5v0: &mut Output<'static>, + en_12v0: &mut Output<'static>, +) { Timer::after_millis(20).await; en_3v3.set_high(); @@ -130,7 +152,11 @@ async fn sequence_power_on(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'st en_12v0.set_high(); } -async fn sequence_power_off(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'static>, en_12v0: &mut Output<'static>) { +async fn sequence_power_off( + en_3v3: &mut Output<'static>, + en_5v0: &mut Output<'static>, + en_12v0: &mut Output<'static>, +) { en_12v0.set_low(); Timer::after_millis(100).await; diff --git a/power-board/src/bin/song-test/main.rs b/power-board/src/bin/song-test/main.rs index ea670b00..07f24122 100644 --- a/power-board/src/bin/song-test/main.rs +++ b/power-board/src/bin/song-test/main.rs @@ -8,12 +8,12 @@ use ateam_lib_stm32::audio::songs::SongId; use ateam_lib_stm32::audio::AudioCommand; use ateam_power_board::pins::{AudioPubSub, TelemetryPubSub}; use ateam_power_board::power_state::SharedPowerState; -use ateam_power_board::{create_power_task, create_coms_task, create_audio_task}; +use ateam_power_board::{create_audio_task, create_coms_task, create_power_task}; use defmt::*; use embassy_executor::{InterruptExecutor, Spawner}; +use embassy_stm32::gpio::{Input, Level, Output, OutputOpenDrain, Pull, Speed}; use embassy_stm32::interrupt; use embassy_stm32::interrupt::{InterruptExt, Priority}; -use embassy_stm32::gpio::{Input, Level, Output, OutputOpenDrain, Pull, Speed}; use embassy_sync::pubsub::PubSubChannel; use embassy_time::{Duration, Instant, Ticker, Timer}; use {defmt_rtt as _, panic_probe as _}; @@ -31,7 +31,6 @@ unsafe fn USART2() { UART_QUEUE_EXECUTOR.on_interrupt(); } - #[embassy_executor::main] async fn main(spawner: Spawner) { let p = embassy_stm32::init(Default::default()); @@ -67,17 +66,31 @@ async fn main(spawner: Spawner) { let power_audio_subscriber = AUDIO_PUBSUB.subscriber().unwrap(); // start power task - create_power_task!(spawner, shared_power_state, power_telemetry_publisher, power_audio_publisher, p); + create_power_task!( + spawner, + shared_power_state, + power_telemetry_publisher, + power_audio_publisher, + p + ); // start coms task - create_coms_task!(spawner, uart_queue_spawner, shared_power_state, coms_telemetry_subscriber, coms_audio_publisher, p); + create_coms_task!( + spawner, + uart_queue_spawner, + shared_power_state, + coms_telemetry_subscriber, + coms_audio_publisher, + p + ); // start audio task create_audio_task!(spawner, power_audio_subscriber, p); - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::PowerOn)).await; + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::PowerOn)) + .await; - - let song_list : [AudioCommand; 9] = [ + let song_list: [AudioCommand; 9] = [ AudioCommand::PlaySong(SongId::Test), AudioCommand::PlaySong(SongId::PowerOn), AudioCommand::PlaySong(SongId::BalanceConnected), @@ -86,7 +99,7 @@ async fn main(spawner: Spawner) { AudioCommand::PlaySong(SongId::ShutdownRequested), AudioCommand::PlaySong(SongId::BatteryLow), AudioCommand::PlaySong(SongId::BatteryCritical), - AudioCommand::PlaySong(SongId::BatteryUhOh) + AudioCommand::PlaySong(SongId::BatteryUhOh), ]; let mut song_num: usize = 0; @@ -99,31 +112,36 @@ async fn main(spawner: Spawner) { // read pwr button // shortest possible interrupt from pwr btn controller is 32ms if pwr_btn.get_level() == Level::Low || SHARED_POWER_STATE.get_shutdown_requested().await { - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::ShutdownRequested)).await; + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::ShutdownRequested)) + .await; Timer::after_millis(250).await; - shutdown_ind.set_low(); // indicate shutdown request + shutdown_ind.set_low(); // indicate shutdown request let shutdown_requested_time = Instant::now(); - SHARED_POWER_STATE.set_shutdown_requested(true).await; // update power board state + SHARED_POWER_STATE.set_shutdown_requested(true).await; // update power board state loop { let cur_robot_state = shared_power_state.get_state().await; - + if !SHARED_POWER_STATE.get_shutdown_requested().await { defmt::info!("MAIN TASK: Shutdown cancelled"); shutdown_ind.set_high(); - break + break; } - if SHARED_POWER_STATE.get_shutdown_ready().await - || Instant::now() > shutdown_requested_time + Duration::from_secs(30) - || !cur_robot_state.coms_established { - main_audio_publisher.publish(AudioCommand::PlaySong(SongId::PowerOff)).await; + if SHARED_POWER_STATE.get_shutdown_ready().await + || Instant::now() > shutdown_requested_time + Duration::from_secs(30) + || !cur_robot_state.coms_established + { + main_audio_publisher + .publish(AudioCommand::PlaySong(SongId::PowerOff)) + .await; // Wait long enough for the song to play :) Timer::after_millis(500).await; defmt::info!("MAIN TASK: Shutdown acknowledged, turning off power"); Timer::after_millis(500).await; sequence_power_off(&mut en_3v3, &mut en_5v0, &mut en_12v0).await; kill_sig.set_low(); - break + break; } Timer::after_millis(10).await; @@ -134,7 +152,11 @@ async fn main(spawner: Spawner) { } } -async fn sequence_power_on(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'static>, en_12v0: &mut Output<'static>) { +async fn sequence_power_on( + en_3v3: &mut Output<'static>, + en_5v0: &mut Output<'static>, + en_12v0: &mut Output<'static>, +) { Timer::after_millis(20).await; en_3v3.set_high(); @@ -145,7 +167,11 @@ async fn sequence_power_on(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'st en_12v0.set_high(); } -async fn sequence_power_off(en_3v3: &mut Output<'static>, en_5v0: &mut Output<'static>, en_12v0: &mut Output<'static>) { +async fn sequence_power_off( + en_3v3: &mut Output<'static>, + en_5v0: &mut Output<'static>, + en_12v0: &mut Output<'static>, +) { en_12v0.set_low(); Timer::after_millis(100).await; diff --git a/power-board/src/config.rs b/power-board/src/config.rs index 474fdb07..9f8e8c00 100644 --- a/power-board/src/config.rs +++ b/power-board/src/config.rs @@ -3,9 +3,14 @@ // criticals are reported to software and force halt high current operations (driving, dribbling, kick/chip charging) // powerdowns result in an automatic initiation software shutdown -use ateam_lib_stm32::{math::{linear_map::LinearMap, range::Range}, power::{battery::BatteryConfig, model::lipo_model::LIPO_CELL_MAX_VOLTAGE, PowerRailParameters}}; +use ateam_lib_stm32::{ + math::{linear_map::LinearMap, range::Range}, + power::{ + battery::BatteryConfig, model::lipo_model::LIPO_CELL_MAX_VOLTAGE, PowerRailParameters, + }, +}; -// power regulators on-board can take a max of 40, and 60 volts respectively. +// power regulators on-board can take a max of 40, and 60 volts respectively. // stspins can take 45V // kicker can take 40V // the ADC measurement range is 0-36V @@ -24,10 +29,10 @@ pub const POWER_RAIL_BATTERY_PARAMETERS: PowerRailParameters = PowerRailPar max_value_crit: VBATT_TOO_HIGH_CRITICAL, }; -pub const REGULATION_HIGH_WARN_MULT: f32 = 1.05; // tolerate 5% error for warning -pub const REGULATION_HIGH_CRIT_MULT: f32 = 1.10; // tolerate 10% for critical -pub const REGULATION_LOW_WARN_MULT: f32 = 0.95; // tolerate 5% error for warning -pub const REGULATION_LOW_CRIT_MULT: f32 = 0.90; // tolerate 10% for critical +pub const REGULATION_HIGH_WARN_MULT: f32 = 1.05; // tolerate 5% error for warning +pub const REGULATION_HIGH_CRIT_MULT: f32 = 1.10; // tolerate 10% for critical +pub const REGULATION_LOW_WARN_MULT: f32 = 0.95; // tolerate 5% error for warning +pub const REGULATION_LOW_CRIT_MULT: f32 = 0.90; // tolerate 10% for critical // 12v0 pub const POWER_RAIL_12V0_TOO_HIGH_WARN: f32 = 12.0 * REGULATION_HIGH_WARN_MULT; @@ -68,7 +73,6 @@ pub const POWER_RAIL_3V3_PARAMETERS: PowerRailParameters = PowerRailParamet const DUAL_RTOL_1PCT_CIEL: f32 = 1.02; const DUAL_RTOL_1PCT_FLOOR: f32 = 0.98; - // battery pub const LIPO_BALANCE_UNCONNECTED_THRESH: f32 = 0.150; @@ -96,10 +100,28 @@ pub const LIPO_BATTERY_CONFIG_6S: BatteryConfig = BatteryConfig { // LHS ranges from resistor dividers in power/power_mon sch page pub const LIPO6S_BALANCE_RAW_SAMPLES_TO_VOLTAGES: [LinearMap; 6] = [ - LinearMap::new(Range::new(0.0, 2.000), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE)), - LinearMap::new(Range::new(0.0, 1.997), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 2.0)), - LinearMap::new(Range::new(0.0, 1.984), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 3.0)), - LinearMap::new(Range::new(0.0, 2.045), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 4.0)), - LinearMap::new(Range::new(0.0, 1.996), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 5.0)), - LinearMap::new(Range::new(0.0, 2.062), Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 6.0)), -]; \ No newline at end of file + LinearMap::new( + Range::new(0.0, 2.000), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE), + ), + LinearMap::new( + Range::new(0.0, 1.997), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 2.0), + ), + LinearMap::new( + Range::new(0.0, 1.984), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 3.0), + ), + LinearMap::new( + Range::new(0.0, 2.045), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 4.0), + ), + LinearMap::new( + Range::new(0.0, 1.996), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 5.0), + ), + LinearMap::new( + Range::new(0.0, 2.062), + Range::new(0.0, LIPO_CELL_MAX_VOLTAGE * 6.0), + ), +]; diff --git a/power-board/src/lib.rs b/power-board/src/lib.rs index b90935d2..f27a83f3 100644 --- a/power-board/src/lib.rs +++ b/power-board/src/lib.rs @@ -8,12 +8,12 @@ use embassy_stm32::{bind_interrupts, peripherals, usart}; pub mod config; pub mod pins; -pub mod tasks; pub mod power_state; pub mod songs; +pub mod tasks; const DEBUG_UART_QUEUES: bool = false; bind_interrupts!(pub struct SystemIrqs { USART1 => usart::InterruptHandler; -}); \ No newline at end of file +}); diff --git a/power-board/src/pins.rs b/power-board/src/pins.rs index f81e94de..a4afbae3 100644 --- a/power-board/src/pins.rs +++ b/power-board/src/pins.rs @@ -1,7 +1,10 @@ use ateam_common_packets::bindings::BatteryInfo; use ateam_lib_stm32::audio::AudioCommand; use embassy_stm32::peripherals::*; -use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, pubsub::{PubSubChannel, Publisher, Subscriber}}; +use embassy_sync::{ + blocking_mutex::raw::ThreadModeRawMutex, + pubsub::{PubSubChannel, Publisher, Subscriber}, +}; ///////////////////// // power control // @@ -80,9 +83,49 @@ const AUDIO_CHANNEL_NUM_PUBS: usize = 3; const TELEMETRY_CHANNEL_DEPTH: usize = 3; const TELEMETRY_CHANNEL_NUM_SUBS: usize = 1; const TELEMETRY_CHANNEL_NUM_PUBS: usize = 1; -pub type AudioPubSub = PubSubChannel; -pub type AudioPublisher = Publisher<'static, ThreadModeRawMutex, AudioCommand, AUDIO_CHANNEL_DEPTH, AUDIO_CHANNEL_NUM_SUBS, AUDIO_CHANNEL_NUM_PUBS>; -pub type AudioSubscriber = Subscriber<'static, ThreadModeRawMutex, AudioCommand, AUDIO_CHANNEL_DEPTH, AUDIO_CHANNEL_NUM_SUBS, AUDIO_CHANNEL_NUM_PUBS>; -pub type TelemetryPubSub = PubSubChannel; -pub type TelemetryPublisher = Publisher<'static, ThreadModeRawMutex, BatteryInfo, TELEMETRY_CHANNEL_DEPTH, TELEMETRY_CHANNEL_NUM_SUBS, TELEMETRY_CHANNEL_NUM_PUBS>; -pub type TelemetrySubscriber = Subscriber<'static, ThreadModeRawMutex, BatteryInfo, TELEMETRY_CHANNEL_DEPTH, TELEMETRY_CHANNEL_NUM_SUBS, TELEMETRY_CHANNEL_NUM_PUBS>; \ No newline at end of file +pub type AudioPubSub = PubSubChannel< + ThreadModeRawMutex, + AudioCommand, + AUDIO_CHANNEL_DEPTH, + AUDIO_CHANNEL_NUM_SUBS, + AUDIO_CHANNEL_NUM_PUBS, +>; +pub type AudioPublisher = Publisher< + 'static, + ThreadModeRawMutex, + AudioCommand, + AUDIO_CHANNEL_DEPTH, + AUDIO_CHANNEL_NUM_SUBS, + AUDIO_CHANNEL_NUM_PUBS, +>; +pub type AudioSubscriber = Subscriber< + 'static, + ThreadModeRawMutex, + AudioCommand, + AUDIO_CHANNEL_DEPTH, + AUDIO_CHANNEL_NUM_SUBS, + AUDIO_CHANNEL_NUM_PUBS, +>; +pub type TelemetryPubSub = PubSubChannel< + ThreadModeRawMutex, + BatteryInfo, + TELEMETRY_CHANNEL_DEPTH, + TELEMETRY_CHANNEL_NUM_SUBS, + TELEMETRY_CHANNEL_NUM_PUBS, +>; +pub type TelemetryPublisher = Publisher< + 'static, + ThreadModeRawMutex, + BatteryInfo, + TELEMETRY_CHANNEL_DEPTH, + TELEMETRY_CHANNEL_NUM_SUBS, + TELEMETRY_CHANNEL_NUM_PUBS, +>; +pub type TelemetrySubscriber = Subscriber< + 'static, + ThreadModeRawMutex, + BatteryInfo, + TELEMETRY_CHANNEL_DEPTH, + TELEMETRY_CHANNEL_NUM_SUBS, + TELEMETRY_CHANNEL_NUM_PUBS, +>; diff --git a/power-board/src/power_state.rs b/power-board/src/power_state.rs index 263e26e5..aed076b4 100644 --- a/power-board/src/power_state.rs +++ b/power-board/src/power_state.rs @@ -1,6 +1,5 @@ use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; - #[derive(Clone, Copy, PartialEq, Debug)] pub struct PowerState { // power status packet flags @@ -21,28 +20,26 @@ pub struct PowerState { } pub struct SharedPowerState { - inner: Mutex, // STM32G0 doesn't have atomic instruction support, use mutex + inner: Mutex, // STM32G0 doesn't have atomic instruction support, use mutex } impl SharedPowerState { pub const fn new() -> Self { Self { - inner: Mutex::new( - PowerState { - power_ok: false, - power_rail_3v3_ok: false, - power_rail_5v0_ok: false, - power_rail_12v0_ok: false, - battery_low_warn: false, - battery_low_crit: false, - high_current_operations_allowed: true, - shutdown_requested: false, - shutdown_force: false, - shutdown_ready: false, - balance_connected: false, - coms_established: false, - } - ) + inner: Mutex::new(PowerState { + power_ok: false, + power_rail_3v3_ok: false, + power_rail_5v0_ok: false, + power_rail_12v0_ok: false, + battery_low_warn: false, + battery_low_crit: false, + high_current_operations_allowed: true, + shutdown_requested: false, + shutdown_force: false, + shutdown_ready: false, + balance_connected: false, + coms_established: false, + }), } } @@ -183,4 +180,4 @@ impl SharedPowerState { let mut guard = self.inner.lock().await; guard.coms_established = new_val; } -} \ No newline at end of file +} diff --git a/power-board/src/songs.rs b/power-board/src/songs.rs index 24d0aa73..6cb1cafc 100644 --- a/power-board/src/songs.rs +++ b/power-board/src/songs.rs @@ -1,75 +1,196 @@ use ateam_lib_stm32::audio::note::Beat; pub const TEST_SONG: [Beat; 17] = [ - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 523, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 392, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 523, duration: 250_000 }, - - Beat::Note { tone: 392, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 392, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 440, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - Beat::Note { tone: 523, duration: 125_000 }, - Beat::Note { tone: 329, duration: 125_000 }, - - Beat::Note { tone: 392, duration: 1_000_000 }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 523, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 523, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 392, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 440, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 523, + duration: 125_000, + }, + Beat::Note { + tone: 329, + duration: 125_000, + }, + Beat::Note { + tone: 392, + duration: 1_000_000, + }, ]; pub const POWER_ON_SONG: [Beat; 3] = [ - Beat::Note { tone: 330, duration: 250_000}, - Beat::Note { tone: 392, duration: 250_000}, - Beat::Note { tone: 523, duration: 500_000}, + Beat::Note { + tone: 330, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 523, + duration: 500_000, + }, ]; pub const BALANCE_CONNECTED_SONG: [Beat; 2] = [ - Beat::Note { tone: 294, duration: 125_000 }, - Beat::Note { tone: 349, duration: 125_000 } + Beat::Note { + tone: 294, + duration: 125_000, + }, + Beat::Note { + tone: 349, + duration: 125_000, + }, ]; pub const BALANCE_DISCONNECTED_SONG: [Beat; 2] = [ - Beat::Note { tone: 349, duration: 125_000 }, - Beat::Note { tone: 294, duration: 125_000 } + Beat::Note { + tone: 349, + duration: 125_000, + }, + Beat::Note { + tone: 294, + duration: 125_000, + }, ]; pub const POWER_OFF_SONG: [Beat; 3] = [ - Beat::Note { tone: 523, duration: 250_000}, - Beat::Note { tone: 392, duration: 250_000}, - Beat::Note { tone: 330, duration: 500_000}, + Beat::Note { + tone: 523, + duration: 250_000, + }, + Beat::Note { + tone: 392, + duration: 250_000, + }, + Beat::Note { + tone: 330, + duration: 500_000, + }, ]; pub const SHUTDOWN_REQUESTED_SONG: [Beat; 4] = [ - Beat::Note { tone: 330, duration: 500_000}, + Beat::Note { + tone: 330, + duration: 500_000, + }, + Beat::Rest(125_000), + Beat::Note { + tone: 330, + duration: 500_000, + }, Beat::Rest(125_000), - Beat::Note { tone: 330, duration: 500_000}, - Beat::Rest(125_000) ]; pub const BATTERY_LOW_SONG: [Beat; 4] = [ - Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { + tone: 392, + duration: 250_000, + }, Beat::Rest(250_000), - Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { + tone: 392, + duration: 250_000, + }, Beat::Rest(250_000), ]; pub const BATTERY_CRITICAL_SONG: [Beat; 2] = [ - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 466, duration: 250_000 }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 466, + duration: 250_000, + }, ]; pub const BATTERY_UH_OH_SONG: [Beat; 8] = [ - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 311, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 311, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 311, duration: 250_000 }, - Beat::Note { tone: 440, duration: 250_000 }, - Beat::Note { tone: 311, duration: 250_000 }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 311, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 311, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 311, + duration: 250_000, + }, + Beat::Note { + tone: 440, + duration: 250_000, + }, + Beat::Note { + tone: 311, + duration: 250_000, + }, ]; diff --git a/power-board/src/tasks/audio_task.rs b/power-board/src/tasks/audio_task.rs index 585995bb..cf01b375 100644 --- a/power-board/src/tasks/audio_task.rs +++ b/power-board/src/tasks/audio_task.rs @@ -1,17 +1,35 @@ -use ateam_lib_stm32::{audio::{songs::SongId, tone_player::TonePlayer, AudioCommand}, drivers::audio::buzzer::Buzzer}; +use ateam_lib_stm32::{ + audio::{songs::SongId, tone_player::TonePlayer, AudioCommand}, + drivers::audio::buzzer::Buzzer, +}; -use embassy_stm32::{timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}, - gpio::OutputType, time::hz}; use embassy_executor::Spawner; +use embassy_stm32::{ + gpio::OutputType, + time::hz, + timer::{ + simple_pwm::{PwmPin, SimplePwm}, + Channel, + }, +}; use embassy_time::{Duration, Ticker, Timer}; -use crate::{pins::*, songs::{BALANCE_CONNECTED_SONG, BALANCE_DISCONNECTED_SONG, BATTERY_CRITICAL_SONG, BATTERY_LOW_SONG, BATTERY_UH_OH_SONG, POWER_OFF_SONG, POWER_ON_SONG, SHUTDOWN_REQUESTED_SONG, TEST_SONG}}; +use crate::{ + pins::*, + songs::{ + BALANCE_CONNECTED_SONG, BALANCE_DISCONNECTED_SONG, BATTERY_CRITICAL_SONG, BATTERY_LOW_SONG, + BATTERY_UH_OH_SONG, POWER_OFF_SONG, POWER_ON_SONG, SHUTDOWN_REQUESTED_SONG, TEST_SONG, + }, +}; #[macro_export] macro_rules! create_audio_task { ($spawner:ident, $audio_subscriber: ident, $p:ident) => { ateam_power_board::tasks::audio_task::start_audio_task( - &$spawner, $audio_subscriber, $p.PC6, $p.TIM3 + &$spawner, + $audio_subscriber, + $p.PC6, + $p.TIM3, ); }; } @@ -33,47 +51,47 @@ async fn audio_task_entry( defmt::warn!("test song"); let _ = tone_player.load_song(&TEST_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::PowerOn) => { defmt::warn!("power on song"); let _ = tone_player.load_song(&POWER_ON_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::PowerOff) => { defmt::warn!("power off song"); let _ = tone_player.load_song(&POWER_OFF_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::ShutdownRequested) => { defmt::warn!("shutdown requested song"); let _ = tone_player.load_song(&SHUTDOWN_REQUESTED_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::BatteryLow) => { defmt::warn!("battery low song"); let _ = tone_player.load_song(&BATTERY_LOW_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::BatteryCritical) => { defmt::warn!("battery critical song"); let _ = tone_player.load_song(&BATTERY_CRITICAL_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::BatteryUhOh) => { defmt::warn!("battery uh oh!!!!!"); let _ = tone_player.load_song(&BATTERY_UH_OH_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::BalanceConnected) => { defmt::warn!("balance connected song"); let _ = tone_player.load_song(&BALANCE_CONNECTED_SONG); tone_player.play_song().await; - }, + } AudioCommand::PlaySong(SongId::BalanceDisconnected) => { defmt::warn!("balance disconnected song"); let _ = tone_player.load_song(&BALANCE_DISCONNECTED_SONG); tone_player.play_song().await; - }, + } } Timer::after_millis(500).await; @@ -84,20 +102,27 @@ async fn audio_task_entry( } pub fn start_audio_task( - spawner: &Spawner, - audio_subscriber: AudioSubscriber, - buzzer_pin: BuzzerPwmPin, - buzzer_timer: BuzzerTimer - ) { + spawner: &Spawner, + audio_subscriber: AudioSubscriber, + buzzer_pin: BuzzerPwmPin, + buzzer_timer: BuzzerTimer, +) { let ch1 = PwmPin::new_ch1(buzzer_pin, OutputType::PushPull); - let pwm = SimplePwm::new(buzzer_timer, Some(ch1), None, None, None, hz(1), Default::default()); + let pwm = SimplePwm::new( + buzzer_timer, + Some(ch1), + None, + None, + None, + hz(1), + Default::default(), + ); // init tone player let audio_driver = Buzzer::new(pwm, Channel::Ch1); let tone_player = TonePlayer::new(audio_driver); - spawner.spawn(audio_task_entry( - tone_player, - audio_subscriber - )).expect("failed to spawn audio task"); -} \ No newline at end of file + spawner + .spawn(audio_task_entry(tone_player, audio_subscriber)) + .expect("failed to spawn audio task"); +} diff --git a/power-board/src/tasks/coms_task.rs b/power-board/src/tasks/coms_task.rs index b7cd756f..db9e7531 100644 --- a/power-board/src/tasks/coms_task.rs +++ b/power-board/src/tasks/coms_task.rs @@ -2,16 +2,27 @@ use core::mem::MaybeUninit; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::{self, DataBits, Parity, StopBits, Uart}; -use ateam_lib_stm32::{audio::{songs::SongId, AudioCommand}, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}}; +use crate::{pins::*, power_state::SharedPowerState, SystemIrqs, DEBUG_UART_QUEUES}; use ateam_common_packets::bindings::{BatteryInfo, PowerCommand, PowerTelemetry}; +use ateam_lib_stm32::{ + audio::{songs::SongId, AudioCommand}, + idle_buffered_uart_spawn_tasks, static_idle_buffered_uart_nl, + uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, +}; use embassy_time::{Duration, Instant, Ticker}; -use crate::{pins::*, power_state::SharedPowerState, SystemIrqs, DEBUG_UART_QUEUES}; const MAX_RX_PACKET_SIZE: usize = core::mem::size_of::(); const MAX_TX_PACKET_SIZE: usize = core::mem::size_of::(); const RX_BUF_DEPTH: usize = 3; const TX_BUF_DEPTH: usize = 2; -static_idle_buffered_uart_nl!(COMS, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, DEBUG_UART_QUEUES); +static_idle_buffered_uart_nl!( + COMS, + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + DEBUG_UART_QUEUES +); const POWER_TELEM_PERIOD_MS: u64 = 100; @@ -21,20 +32,35 @@ const COMS_RX_TIMEOUT: Duration = Duration::from_millis(1000); macro_rules! create_coms_task { ($main_spawner:ident, $uart_queue_spawner:ident, $shared_power_state:ident, $coms_telemetry_subscriber:ident, $coms_audio_publisher: ident, $p:ident) => { ateam_power_board::tasks::coms_task::start_coms_task( - $main_spawner, $uart_queue_spawner, $shared_power_state, $coms_telemetry_subscriber, $coms_audio_publisher, - $p.USART1, $p.PA10, $p.PA9, $p.DMA1_CH5, $p.DMA1_CH4, - ).await; + $main_spawner, + $uart_queue_spawner, + $shared_power_state, + $coms_telemetry_subscriber, + $coms_audio_publisher, + $p.USART1, + $p.PA10, + $p.PA9, + $p.DMA1_CH5, + $p.DMA1_CH4, + ) + .await; }; } #[embassy_executor::task] async fn coms_task_entry( - _uart: &'static IdleBufferedUart, + _uart: &'static IdleBufferedUart< + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + DEBUG_UART_QUEUES, + >, read_queue: &'static UartReadQueue, write_queue: &'static UartWriteQueue, shared_power_state: &'static SharedPowerState, mut telemetry_subscriber: TelemetrySubscriber, - audio_publisher: AudioPublisher + audio_publisher: AudioPublisher, ) { let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); let mut last_battery_telem_packet: BatteryInfo = Default::default(); @@ -49,7 +75,12 @@ async fn coms_task_entry( let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("COMS TASK - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); + defmt::warn!( + "COMS TASK - Got invalid packet of len {:?} (expected {:?}) data: {:?}", + buf.len(), + core::mem::size_of::(), + buf + ); continue; } @@ -79,7 +110,7 @@ async fn coms_task_entry( defmt::info!("COMS TASK - force shutdown received from control board"); shared_power_state.set_shutdown_force(true).await; } - + if command_packet.ready_shutdown() != 0 { defmt::info!("COMS TASK - ready shutdown received from control board"); shared_power_state.set_shutdown_ready(true).await; @@ -87,7 +118,9 @@ async fn coms_task_entry( if command_packet.request_shutdown() != 0 { defmt::info!("COMS TASK - request shutdown received from control board"); - audio_publisher.publish(AudioCommand::PlaySong(SongId::ShutdownRequested)).await; + audio_publisher + .publish(AudioCommand::PlaySong(SongId::ShutdownRequested)) + .await; shared_power_state.set_shutdown_requested(true).await; } @@ -103,16 +136,16 @@ async fn coms_task_entry( defmt::trace!("COMS TASK - New battery telemetry packet received from power task"); last_battery_telem_packet = telemetry_packet; } - // if we've timed out coms, set it in the state - if cur_power_state.coms_established && Instant::now() > last_received_packet_time + COMS_RX_TIMEOUT { + if cur_power_state.coms_established + && Instant::now() > last_received_packet_time + COMS_RX_TIMEOUT + { shared_power_state.set_coms_established(false).await; } // Send telemetry to control board if Instant::now() - last_sent_packet_time >= Duration::from_millis(POWER_TELEM_PERIOD_MS) { - // build telemetry packet from latest battery telemetry and power status let mut telem_packet: PowerTelemetry = Default::default(); @@ -120,7 +153,9 @@ async fn coms_task_entry( telem_packet.set_power_rail_3v3_ok(cur_power_state.power_rail_3v3_ok as u32); telem_packet.set_power_rail_5v0_ok(cur_power_state.power_rail_5v0_ok as u32); telem_packet.set_power_rail_12v0_ok(cur_power_state.power_rail_12v0_ok as u32); - telem_packet.set_high_current_operations_allowed(cur_power_state.high_current_operations_allowed as u32); + telem_packet.set_high_current_operations_allowed( + cur_power_state.high_current_operations_allowed as u32, + ); telem_packet.set_shutdown_requested(cur_power_state.shutdown_requested as u32); telem_packet.battery_info = last_battery_telem_packet; @@ -151,7 +186,7 @@ pub fn power_uart_config() -> usart::Config { } pub async fn start_coms_task( - spawner: Spawner, + spawner: Spawner, uart_queue_spawner: SendSpawner, shared_power_state: &'static SharedPowerState, telemetry_subscriber: TelemetrySubscriber, @@ -160,13 +195,29 @@ pub async fn start_coms_task( uart_rx_pin: ComsUartRxPin, uart_tx_pin: ComsUartTxPin, uart_rx_dma: ComsDmaRx, - uart_tx_dma: ComsDmaTx + uart_tx_dma: ComsDmaTx, ) { let uart_config = power_uart_config(); - let coms_uart = Uart::new(uart, uart_rx_pin, uart_tx_pin, SystemIrqs, uart_tx_dma, uart_rx_dma, uart_config).unwrap(); + let coms_uart = Uart::new( + uart, + uart_rx_pin, + uart_tx_pin, + SystemIrqs, + uart_tx_dma, + uart_rx_dma, + uart_config, + ) + .unwrap(); COMS_IDLE_BUFFERED_UART.init(); idle_buffered_uart_spawn_tasks!(uart_queue_spawner, COMS, coms_uart); - spawner.spawn(coms_task_entry( - &COMS_IDLE_BUFFERED_UART, &COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), &COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), shared_power_state, telemetry_subscriber, audio_publisher - )).expect("failed to spawn coms task"); -} \ No newline at end of file + spawner + .spawn(coms_task_entry( + &COMS_IDLE_BUFFERED_UART, + &COMS_IDLE_BUFFERED_UART.get_uart_read_queue(), + &COMS_IDLE_BUFFERED_UART.get_uart_write_queue(), + shared_power_state, + telemetry_subscriber, + audio_publisher, + )) + .expect("failed to spawn coms task"); +} diff --git a/power-board/src/tasks/io_task.rs b/power-board/src/tasks/io_task.rs index e69de29b..8b137891 100644 --- a/power-board/src/tasks/io_task.rs +++ b/power-board/src/tasks/io_task.rs @@ -0,0 +1 @@ + diff --git a/power-board/src/tasks/led_task.rs b/power-board/src/tasks/led_task.rs index 0a08c573..3e0fc8f0 100644 --- a/power-board/src/tasks/led_task.rs +++ b/power-board/src/tasks/led_task.rs @@ -21,10 +21,10 @@ async fn led_task_entry() { } } -pub async fn start_power_task(spawner: &Spawner, - ) { - +pub async fn start_power_task(spawner: &Spawner) { // init Apa102 animation stuff - spawner.spawn(led_task_entry()).expect("failed to spawn led task"); -} \ No newline at end of file + spawner + .spawn(led_task_entry()) + .expect("failed to spawn led task"); +} diff --git a/power-board/src/tasks/mod.rs b/power-board/src/tasks/mod.rs index 56208768..8827090e 100644 --- a/power-board/src/tasks/mod.rs +++ b/power-board/src/tasks/mod.rs @@ -2,4 +2,4 @@ pub mod audio_task; pub mod coms_task; pub mod io_task; pub mod led_task; -pub mod power_task; \ No newline at end of file +pub mod power_task; diff --git a/power-board/src/tasks/power_task.rs b/power-board/src/tasks/power_task.rs index d8c3359a..7159a4a3 100644 --- a/power-board/src/tasks/power_task.rs +++ b/power-board/src/tasks/power_task.rs @@ -1,10 +1,30 @@ use ateam_common_packets::bindings::BatteryInfo; -use ateam_lib_stm32::{audio::{songs::SongId, AudioCommand}, drivers::adc::AdcConverter, filter::WindowAvergingFilter, math::range::Range, power::{battery::LipoModel, model::lipo_model::{lipo_pct_interp, LIPO_6S_VOLTAGE_PERCENT}, PowerRail}}; +use ateam_lib_stm32::{ + audio::{songs::SongId, AudioCommand}, + drivers::adc::AdcConverter, + filter::WindowAvergingFilter, + math::range::Range, + power::{ + battery::LipoModel, + model::lipo_model::{lipo_pct_interp, LIPO_6S_VOLTAGE_PERCENT}, + PowerRail, + }, +}; use embassy_executor::Spawner; -use embassy_stm32::{adc::{Adc, AdcChannel, AnyAdcChannel, SampleTime}, peripherals::ADC1}; +use embassy_stm32::{ + adc::{Adc, AdcChannel, AnyAdcChannel, SampleTime}, + peripherals::ADC1, +}; use embassy_time::{Duration, Instant, Ticker, Timer}; -use crate::{config::{LIPO6S_BALANCE_RAW_SAMPLES_TO_VOLTAGES, LIPO_BATTERY_CONFIG_6S, POWER_RAIL_12V0_PARAMETERS, POWER_RAIL_3V3_PARAMETERS, POWER_RAIL_5V0_PARAMETERS, POWER_RAIL_BATTERY_PARAMETERS}, pins::*, power_state::SharedPowerState}; +use crate::{ + config::{ + LIPO6S_BALANCE_RAW_SAMPLES_TO_VOLTAGES, LIPO_BATTERY_CONFIG_6S, POWER_RAIL_12V0_PARAMETERS, + POWER_RAIL_3V3_PARAMETERS, POWER_RAIL_5V0_PARAMETERS, POWER_RAIL_BATTERY_PARAMETERS, + }, + pins::*, + power_state::SharedPowerState, +}; const BATTERY_CELL_READ_INTERVAL: Duration = Duration::from_millis(1300); const POWER_RAIL_FILTER_WINDOW_SIZE: usize = 10; @@ -13,10 +33,25 @@ const POWER_RAIL_FILTER_WINDOW_SIZE: usize = 10; macro_rules! create_power_task { ($spawner:ident, $shared_power_state:ident, $telemetry_publisher:ident, $audio_publisher:ident, $p:ident) => { ateam_power_board::tasks::power_task::start_power_task( - &$spawner, $shared_power_state, $telemetry_publisher, $audio_publisher, - $p.ADC1, $p.DMA1_CH1, - $p.PA0, $p.PA1, $p.PA2, $p.PA3, $p.PA4, $p.PA5, - $p.PB1, $p.PA7, $p.PA6, $p.PB10, $p.PB2).await; + &$spawner, + $shared_power_state, + $telemetry_publisher, + $audio_publisher, + $p.ADC1, + $p.DMA1_CH1, + $p.PA0, + $p.PA1, + $p.PA2, + $p.PA3, + $p.PA4, + $p.PA5, + $p.PB1, + $p.PA7, + $p.PA6, + $p.PB10, + $p.PB2, + ) + .await; }; } @@ -53,7 +88,7 @@ async fn power_task_entry( let mut cell4_adc_pin = cell4_adc_pin.degrade_adc(); let mut cell5_adc_pin = cell5_adc_pin.degrade_adc(); let mut cell6_adc_pin = cell6_adc_pin.degrade_adc(); - let mut power_rail_12v0_adc_pin = power_rail_12v0_adc_pin.degrade_adc(); + let mut power_rail_12v0_adc_pin = power_rail_12v0_adc_pin.degrade_adc(); let mut power_rail_5v0_adc_pin = power_rail_5v0_adc_pin.degrade_adc(); let mut power_rail_3v3_adc_pin = power_rail_3v3_adc_pin.degrade_adc(); let mut power_rail_vbatt_before_lsw_adc_pin = power_rail_vbatt_before_lsw_adc_pin.degrade_adc(); @@ -78,36 +113,45 @@ async fn power_task_entry( //////////////////////// // Power Rail Setup // //////////////////////// - - let mut power_rail_battery = PowerRail::new(POWER_RAIL_BATTERY_PARAMETERS, + + let mut power_rail_battery = PowerRail::new( + POWER_RAIL_BATTERY_PARAMETERS, WindowAvergingFilter::::new(), Range::new(0.0, 2062.0), - Range::new(0.0, 25.2)); + Range::new(0.0, 25.2), + ); - let mut power_rail_12v0 = PowerRail::new(POWER_RAIL_12V0_PARAMETERS, + let mut power_rail_12v0 = PowerRail::new( + POWER_RAIL_12V0_PARAMETERS, WindowAvergingFilter::::new(), Range::new(0.0, 1741.0), - Range::new(0.0, 12.0)); + Range::new(0.0, 12.0), + ); - let mut power_rail_5v0 = PowerRail::new(POWER_RAIL_5V0_PARAMETERS, + let mut power_rail_5v0 = PowerRail::new( + POWER_RAIL_5V0_PARAMETERS, WindowAvergingFilter::::new(), Range::new(0.0, 1745.0), - Range::new(0.0, 5.0)); + Range::new(0.0, 5.0), + ); - let mut power_rail_3v3 = PowerRail::new(POWER_RAIL_3V3_PARAMETERS, + let mut power_rail_3v3 = PowerRail::new( + POWER_RAIL_3V3_PARAMETERS, WindowAvergingFilter::::new(), Range::new(0.0, 1750.0), - Range::new(0.0, 3.3)); + Range::new(0.0, 3.3), + ); /////////////////////////// // Battery Model Setup // /////////////////////////// - let mut lipo6s_battery_model: LipoModel<'_, 6, WindowAvergingFilter<15, true, f32>> = - LipoModel::new( - LIPO_BATTERY_CONFIG_6S, - &LIPO6S_BALANCE_RAW_SAMPLES_TO_VOLTAGES, - ateam_lib_stm32::power::battery::CellVoltageComputeMode::Chained); + let mut lipo6s_battery_model: LipoModel<'_, 6, WindowAvergingFilter<15, true, f32>> = + LipoModel::new( + LIPO_BATTERY_CONFIG_6S, + &LIPO6S_BALANCE_RAW_SAMPLES_TO_VOLTAGES, + ateam_lib_stm32::power::battery::CellVoltageComputeMode::Chained, + ); // Create empty telemetry packet let mut battery_telem_packet: BatteryInfo = Default::default(); @@ -127,36 +171,55 @@ async fn power_task_entry( (&mut power_rail_5v0_adc_pin, SampleTime::CYCLES160_5), (&mut power_rail_3v3_adc_pin, SampleTime::CYCLES160_5), (&mut vrefint_channel, SampleTime::CYCLES160_5), - ].into_iter(); - adc.read(&mut adc_dma, power_rail_read_seq, &mut power_rail_adc_raw_samples).await; + ] + .into_iter(); + adc.read( + &mut adc_dma, + power_rail_read_seq, + &mut power_rail_adc_raw_samples, + ) + .await; // covert power rails // let power_rail_adc_voltages = PowerRailAdcSamples::new_from_samples(&power_rail_adc_raw_samples).expect("invalid slice length on power rail adc sample conversion"); adc_converter.update_vrefint(*power_rail_adc_raw_samples.last().unwrap_or(&0)); - adc_converter.raw_samples_to_mv(&power_rail_adc_raw_samples, &mut power_rail_voltage_samples); + adc_converter + .raw_samples_to_mv(&power_rail_adc_raw_samples, &mut power_rail_voltage_samples); power_rail_3v3.add_rail_voltage_sample(power_rail_voltage_samples[0] as f32); power_rail_5v0.add_rail_voltage_sample(power_rail_voltage_samples[1] as f32); power_rail_12v0.add_rail_voltage_sample(power_rail_voltage_samples[2] as f32); power_rail_battery.add_rail_voltage_sample(power_rail_voltage_samples[3] as f32); - defmt::info!("power rail voltages: Battery {}, 12v0 {}, 5v0 {}, 3v3 {}", power_rail_battery.get_rail_voltage(), power_rail_12v0.get_rail_voltage(), power_rail_5v0.get_rail_voltage(), power_rail_3v3.get_rail_voltage()); + defmt::info!( + "power rail voltages: Battery {}, 12v0 {}, 5v0 {}, 3v3 {}", + power_rail_battery.get_rail_voltage(), + power_rail_12v0.get_rail_voltage(), + power_rail_5v0.get_rail_voltage(), + power_rail_3v3.get_rail_voltage() + ); /////////////////////////////// // Check Power Rail Errors // /////////////////////////////// - - shared_power_state.set_power_rail_3v3_ok(power_rail_3v3.power_ok()).await; - shared_power_state.set_power_rail_5v0_ok(power_rail_5v0.power_ok()).await; - shared_power_state.set_power_rail_12v0_ok(power_rail_12v0.power_ok()).await; - shared_power_state.set_power_ok( - power_rail_3v3.power_ok() && - power_rail_5v0.power_ok() && - power_rail_12v0.power_ok() && - power_rail_battery.power_ok() - ).await; - + shared_power_state + .set_power_rail_3v3_ok(power_rail_3v3.power_ok()) + .await; + shared_power_state + .set_power_rail_5v0_ok(power_rail_5v0.power_ok()) + .await; + shared_power_state + .set_power_rail_12v0_ok(power_rail_12v0.power_ok()) + .await; + shared_power_state + .set_power_ok( + power_rail_3v3.power_ok() + && power_rail_5v0.power_ok() + && power_rail_12v0.power_ok() + && power_rail_battery.power_ok(), + ) + .await; ////////////////////////// // Battery Monitoring // @@ -170,28 +233,55 @@ async fn power_task_entry( (&mut cell4_adc_pin, SampleTime::CYCLES160_5), (&mut cell5_adc_pin, SampleTime::CYCLES160_5), (&mut cell6_adc_pin, SampleTime::CYCLES160_5), - (&mut power_rail_vbatt_before_lsw_adc_pin, SampleTime::CYCLES160_5), + ( + &mut power_rail_vbatt_before_lsw_adc_pin, + SampleTime::CYCLES160_5, + ), (&mut vrefint_channel, SampleTime::CYCLES160_5), - ].into_iter(); - adc.read(&mut adc_dma, battery_cell_read_seq, &mut battery_cell_adc_raw_samples).await; + ] + .into_iter(); + adc.read( + &mut adc_dma, + battery_cell_read_seq, + &mut battery_cell_adc_raw_samples, + ) + .await; last_battery_cell_read_time = Instant::now(); adc_converter.update_vrefint(*battery_cell_adc_raw_samples.last().unwrap_or(&0)); - adc_converter.raw_samples_to_v(&battery_cell_adc_raw_samples, &mut battery_cell_voltage_samples); + adc_converter.raw_samples_to_v( + &battery_cell_adc_raw_samples, + &mut battery_cell_voltage_samples, + ); lipo6s_battery_model.add_cell_voltage_samples(&battery_cell_voltage_samples[0..6]); - defmt::info!("battery cell voltages {}", lipo6s_battery_model.get_cell_voltages()); - defmt::info!("battery cell percentages {}", lipo6s_battery_model.get_cell_percentages()); - defmt::info!("battery worst cell imblanace {}", lipo6s_battery_model.get_worst_cell_imbalance()); - - if lipo6s_battery_model.get_cell_percentages().into_iter().all(|v| *v <= 0) { + defmt::info!( + "battery cell voltages {}", + lipo6s_battery_model.get_cell_voltages() + ); + defmt::info!( + "battery cell percentages {}", + lipo6s_battery_model.get_cell_percentages() + ); + defmt::info!( + "battery worst cell imblanace {}", + lipo6s_battery_model.get_worst_cell_imbalance() + ); + + if lipo6s_battery_model + .get_cell_percentages() + .into_iter() + .all(|v| *v <= 0) + { if cur_power_state.balance_connected { // the balance connection is lost // set LED // play song - audio_publisher.publish(AudioCommand::PlaySong(SongId::BalanceDisconnected)).await; + audio_publisher + .publish(AudioCommand::PlaySong(SongId::BalanceDisconnected)) + .await; } defmt::info!("battery balance connector is unplugged"); @@ -201,36 +291,49 @@ async fn power_task_entry( // the balance connection is made for the first time // set LED // play song - audio_publisher.publish(AudioCommand::PlaySong(SongId::BalanceConnected)).await; + audio_publisher + .publish(AudioCommand::PlaySong(SongId::BalanceConnected)) + .await; } shared_power_state.set_balance_connected(true).await; } - + // Vbatt PMIC differential - if f32::abs(battery_cell_voltage_samples[7] - power_rail_voltage_samples[3] as f32 / 1000.0) > 1.0 { + if f32::abs( + battery_cell_voltage_samples[7] - power_rail_voltage_samples[3] as f32 / 1000.0, + ) > 1.0 + { defmt::error!("adc measuring substantial voltage drop across the load switch"); } let mut high_current_ops_allowed = true; - if power_rail_battery.warning_flagged() || (cur_power_state.balance_connected && lipo6s_battery_model.battery_warn()) { + if power_rail_battery.warning_flagged() + || (cur_power_state.balance_connected && lipo6s_battery_model.battery_warn()) + { if !cur_power_state.battery_low_warn { // we've entered low power state for the first time // set led // play song - audio_publisher.publish(AudioCommand::PlaySong(SongId::BatteryLow)).await; + audio_publisher + .publish(AudioCommand::PlaySong(SongId::BatteryLow)) + .await; } shared_power_state.set_battery_low_warn(true).await; } - if power_rail_battery.critical_warning_flagged() || (cur_power_state.balance_connected && lipo6s_battery_model.battery_crit()) { + if power_rail_battery.critical_warning_flagged() + || (cur_power_state.balance_connected && lipo6s_battery_model.battery_crit()) + { if !cur_power_state.battery_low_crit { // we've entered low critical power state for the first time // set led // play song - audio_publisher.publish(AudioCommand::PlaySong(SongId::BatteryCritical)).await; + audio_publisher + .publish(AudioCommand::PlaySong(SongId::BatteryCritical)) + .await; } shared_power_state.set_battery_low_crit(true).await; @@ -242,19 +345,29 @@ async fn power_task_entry( high_current_ops_allowed = false; } - shared_power_state.set_high_current_operations_allowed(high_current_ops_allowed).await; + shared_power_state + .set_high_current_operations_allowed(high_current_ops_allowed) + .await; } battery_telem_packet.set_battery_ok(lipo6s_battery_model.battery_ok() as u16); - battery_telem_packet.set_battery_balance_connected(cur_power_state.balance_connected as u16); + battery_telem_packet + .set_battery_balance_connected(cur_power_state.balance_connected as u16); battery_telem_packet.set_battery_low(power_rail_battery.warning_flagged() as u16); - battery_telem_packet.set_battery_critical(power_rail_battery.critical_warning_flagged() as u16); + battery_telem_packet + .set_battery_critical(power_rail_battery.critical_warning_flagged() as u16); battery_telem_packet.set_battery_cell_low(lipo6s_battery_model.battery_warn() as u16); battery_telem_packet.set_battery_cell_critical(lipo6s_battery_model.battery_crit() as u16); - battery_telem_packet.set_battery_cell_imbalance_warn(lipo6s_battery_model.battery_cell_imbalance_warn() as u16); + battery_telem_packet.set_battery_cell_imbalance_warn( + lipo6s_battery_model.battery_cell_imbalance_warn() as u16, + ); - battery_telem_packet.battery_mv = (power_rail_battery.get_rail_voltage().unwrap_or(0.0) / 1000.0) as u16; - battery_telem_packet.battery_pct = lipo_pct_interp(power_rail_battery.get_rail_voltage().unwrap_or(0.0), &LIPO_6S_VOLTAGE_PERCENT) as u8; + battery_telem_packet.battery_mv = + (power_rail_battery.get_rail_voltage().unwrap_or(0.0) / 1000.0) as u16; + battery_telem_packet.battery_pct = lipo_pct_interp( + power_rail_battery.get_rail_voltage().unwrap_or(0.0), + &LIPO_6S_VOLTAGE_PERCENT, + ) as u8; battery_telem_packet.cell1_mv = lipo6s_battery_model.get_cell_mv(0); battery_telem_packet.cell2_mv = lipo6s_battery_model.get_cell_mv(1); @@ -270,7 +383,6 @@ async fn power_task_entry( battery_telem_packet.cell5_pct = lipo6s_battery_model.get_cell_pct(4); battery_telem_packet.cell6_pct = lipo6s_battery_model.get_cell_pct(5); - // send pubsub message to coms task telemetry_publisher.publish_immediate(battery_telem_packet); @@ -278,7 +390,8 @@ async fn power_task_entry( } } -pub async fn start_power_task(spawner: &Spawner, +pub async fn start_power_task( + spawner: &Spawner, shared_power_state: &'static SharedPowerState, telemetry_publisher: TelemetryPublisher, audio_publisher: AudioPublisher, @@ -295,13 +408,25 @@ pub async fn start_power_task(spawner: &Spawner, power_rail_3v3_adc_pin: Power3v3VoltageMonitorPin, power_rail_vbatt_before_lsw_adc_pin: BatteryPreLoadSwitchVoltageMonitorPin, power_rail_vbatt_adc_pin: BatteryVoltageMonitorPin, - ) { - spawner.spawn(power_task_entry( - shared_power_state, - telemetry_publisher, - audio_publisher, - adc, adc_dma, - cell1_adc_pin, cell2_adc_pin, cell3_adc_pin, cell4_adc_pin, cell5_adc_pin, cell6_adc_pin, - power_rail_12v0_adc_pin, power_rail_5v0_adc_pin, power_rail_3v3_adc_pin, power_rail_vbatt_before_lsw_adc_pin, power_rail_vbatt_adc_pin - )).expect("failed to spawn power task"); -} \ No newline at end of file +) { + spawner + .spawn(power_task_entry( + shared_power_state, + telemetry_publisher, + audio_publisher, + adc, + adc_dma, + cell1_adc_pin, + cell2_adc_pin, + cell3_adc_pin, + cell4_adc_pin, + cell5_adc_pin, + cell6_adc_pin, + power_rail_12v0_adc_pin, + power_rail_5v0_adc_pin, + power_rail_3v3_adc_pin, + power_rail_vbatt_before_lsw_adc_pin, + power_rail_vbatt_adc_pin, + )) + .expect("failed to spawn power task"); +} From 7536627a12876cf40f279ce102b70144c66820e6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:30:33 -0400 Subject: [PATCH 06/22] fetch credentials private --- .github/workflows/rustfmt.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml index e0b3165c..2d89b843 100644 --- a/.github/workflows/rustfmt.yml +++ b/.github/workflows/rustfmt.yml @@ -11,6 +11,14 @@ runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 + - name: Fetch Submodules + uses: webfactory/ssh-agent@v0.7.0 + with: + ssh-private-key: | + ${{ secrets.ROBOT_PRIVATE_DATA_PRIVATE_SSH_KEY }} + - run: | + git submodule update --init --recursive + ./util/ateam-credentials-setup.bash - uses: actions-rust-lang/setup-rust-toolchain@v1 with: toolchain: nightly From ff56eed90376fac6617a9d94bb5e8f5af57b25c1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 22 Aug 2025 17:35:23 -0400 Subject: [PATCH 07/22] make CI style check not quiet --- .github/workflows/rustfmt.yml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml index 2d89b843..6d43ba02 100644 --- a/.github/workflows/rustfmt.yml +++ b/.github/workflows/rustfmt.yml @@ -24,16 +24,16 @@ toolchain: nightly components: rustfmt - name: Common - run: cargo fmt --quiet --check --manifest-path common/Cargo.toml + run: cargo fmt --check --manifest-path common/Cargo.toml - name: Library - run: cargo fmt --quiet --check --manifest-path lib-stm32/Cargo.toml + run: cargo fmt --check --manifest-path lib-stm32/Cargo.toml - name: LIbrary Test Images - run: cargo fmt --quiet --check --manifest-path lib-stm32-test/Cargo.toml + run: cargo fmt --check --manifest-path lib-stm32-test/Cargo.toml - name: Credentials - run: cargo fmt --quiet --check --manifest-path credentials/Cargo.toml + run: cargo fmt --check --manifest-path credentials/Cargo.toml - name: Power Board - run: cargo fmt --quiet --check --manifest-path power-board/Cargo.toml + run: cargo fmt --check --manifest-path power-board/Cargo.toml - name: Kicker Board - run: cargo fmt --quiet --check --manifest-path kicker-board/Cargo.toml + run: cargo fmt --check --manifest-path kicker-board/Cargo.toml - name: Control Board - run: cargo fmt --quiet --check --manifest-path control-board/Cargo.toml \ No newline at end of file + run: cargo fmt --check --manifest-path control-board/Cargo.toml \ No newline at end of file From 19f8f8da0d6b1cf627b507f21537a57dc21eeafc Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 23 Aug 2025 22:47:03 -0400 Subject: [PATCH 08/22] move Nix deps to latest --- flake.lock | 18 +++++++++--------- flake.nix | 6 +++--- motor-controller/common/time.h | 4 +++- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/flake.lock b/flake.lock index 05da64d0..f722c53e 100644 --- a/flake.lock +++ b/flake.lock @@ -20,11 +20,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1737822063, - "narHash": "sha256-lnupSJZcHubdG44NEo3UAV19yEHqYEwc1WfO1iXPGRs=", + "lastModified": 1755900021, + "narHash": "sha256-spSv0dlqAuTz62nZAWxoB2xAiv6ur3nnasuetZaT8No=", "owner": "nixos", "repo": "nixpkgs", - "rev": "ee8068209bc09d8bc7d0055ba1de73401df8a39a", + "rev": "cc17aa283cae8b37804f7f6f0fb9240037063dd3", "type": "github" }, "original": { @@ -35,11 +35,11 @@ }, "nixpkgs_2": { "locked": { - "lastModified": 1736320768, - "narHash": "sha256-nIYdTAiKIGnFNugbomgBJR+Xv5F1ZQU+HfaBqJKroC0=", + "lastModified": 1744536153, + "narHash": "sha256-awS2zRgF4uTwrOKwwiJcByDzDOdo3Q1rPZbiHQg/N38=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "4bc9c909d9ac828a039f288cf872d16d38185db8", + "rev": "18dd725c29603f582cf1900e0d25f9f1063dbf11", "type": "github" }, "original": { @@ -61,11 +61,11 @@ "nixpkgs": "nixpkgs_2" }, "locked": { - "lastModified": 1737771740, - "narHash": "sha256-lWIdF4qke63TdCHnJ0QaUHfG8YvsDrBqzL4jiHYQd+Y=", + "lastModified": 1755830208, + "narHash": "sha256-fMa/Hcg+4O2h+kl3gNPjtGSWPI8NtCl3LYMsejK6qGA=", "owner": "oxalica", "repo": "rust-overlay", - "rev": "cfaaa1dddd280af09aca84af84612fbccd986ae2", + "rev": "802a7b97f8ff672ba2dec70c9e354f51f844e796", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index bbfc3960..89aa21a5 100644 --- a/flake.nix +++ b/flake.nix @@ -37,7 +37,7 @@ # C/C++ build utils gnumake cmake - gcc-arm-embedded-12 + gcc-arm-embedded-14 # programmers dfu-util @@ -48,10 +48,10 @@ clang # Rust Embedded - (rust-bin.nightly."2024-12-10".default.override { + (rust-bin.selectLatestNightlyWith (toolchain: toolchain.default.override { extensions = [ "rust-src" "rust-analyzer" ]; targets = [ "thumbv7em-none-eabihf" "thumbv6m-none-eabi" ]; - }) + })) # Python (pkgs.${python}.withPackages diff --git a/motor-controller/common/time.h b/motor-controller/common/time.h index 9019ecda..fc7285f0 100644 --- a/motor-controller/common/time.h +++ b/motor-controller/common/time.h @@ -86,4 +86,6 @@ bool time_sync_block(SyncTimer_t *time_sync); * @return true * @return false */ -bool time_sync_block_rst(SyncTimer_t *time_sync); \ No newline at end of file +bool time_sync_block_rst(SyncTimer_t *time_sync); + +void wait_cyc(uint32_t cycles); \ No newline at end of file From 4d215cf06984915d31c92eb42aab8334de9600b8 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 24 Aug 2025 18:44:34 -0400 Subject: [PATCH 09/22] move rust deps forward, and fix build --- control-board/Cargo.lock | 502 ++++++++------- control-board/Cargo.toml | 25 +- .../src/bin/hwtest-kicker-coms/main.rs | 4 +- control-board/src/bin/hwtest-kicker/main.rs | 4 +- control-board/src/bin/hwtest-piezo/main.rs | 2 +- control-board/src/drivers/kicker.rs | 8 +- control-board/src/drivers/radio_robot.rs | 6 +- control-board/src/drivers/shell_indicator.rs | 12 +- control-board/src/lib.rs | 2 - control-board/src/pins.rs | 14 +- control-board/src/stspin_motor.rs | 8 +- control-board/src/tasks/audio_task.rs | 8 +- control-board/src/tasks/control_task.rs | 74 +-- control-board/src/tasks/dotstar_task.rs | 9 +- control-board/src/tasks/imu_task.rs | 62 +- control-board/src/tasks/kicker_task.rs | 24 +- control-board/src/tasks/power_task.rs | 12 +- control-board/src/tasks/radio_task.rs | 30 +- control-board/src/tasks/user_io_task.rs | 77 +-- kicker-board/Cargo.lock | 577 +++++++++--------- kicker-board/Cargo.toml | 24 +- kicker-board/src/bin/hwtest-blinky/main.rs | 28 +- kicker-board/src/bin/hwtest-breakbeam/main.rs | 2 +- kicker-board/src/bin/hwtest-charge/main.rs | 17 +- kicker-board/src/bin/hwtest-coms/main.rs | 19 +- kicker-board/src/bin/hwtest-dribbler/main.rs | 4 +- kicker-board/src/bin/hwtest-flash/main.rs | 2 +- kicker-board/src/bin/hwtest-kick/main.rs | 18 +- kicker-board/src/bin/hwtest-kickstr/main.rs | 18 +- kicker-board/src/bin/kicker/main.rs | 27 +- kicker-board/src/drivers/breakbeam.rs | 7 +- kicker-board/src/drivers/mod.rs | 8 +- kicker-board/src/lib.rs | 1 - lib-stm32-test/Cargo.lock | 305 ++++----- lib-stm32-test/Cargo.toml | 21 +- .../src/bin/hwtest-adv-btn-async/main.rs | 1 - .../src/bin/hwtest-adv-btn-poll/main.rs | 1 - .../bin/hwtest-uart-queue-mixedbaud/main.rs | 12 +- .../bin/hwtest-uart-queue-mixedrate/main.rs | 12 +- lib-stm32/Cargo.lock | 169 ++--- lib-stm32/Cargo.toml | 16 +- lib-stm32/src/drivers/boot/stm32_interface.rs | 7 +- lib-stm32/src/drivers/flash/at25df041b.rs | 6 +- lib-stm32/src/drivers/imu/bmi085.rs | 20 +- lib-stm32/src/drivers/imu/bmi323.rs | 19 +- lib-stm32/src/drivers/led/apa102.rs | 11 +- lib-stm32/src/drivers/other/adc_helper.rs | 5 +- lib-stm32/src/drivers/switches/button.rs | 7 +- lib-stm32/src/drivers/switches/dip.rs | 4 +- .../src/drivers/switches/rotary_encoder.rs | 4 +- lib-stm32/src/lib.rs | 1 - lib-stm32/src/uart/queue.rs | 4 + power-board/Cargo.lock | 365 +++++++---- power-board/Cargo.toml | 14 +- power-board/src/bin/hwtest-bringup/main.rs | 4 +- power-board/src/tasks/audio_task.rs | 8 +- power-board/src/tasks/coms_task.rs | 12 +- power-board/src/tasks/power_task.rs | 58 +- 58 files changed, 1431 insertions(+), 1290 deletions(-) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 730c5bdb..447ab01d 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -22,12 +22,15 @@ dependencies = [ [[package]] name = "apa102-spi" -version = "0.4.0" +version = "0.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "747bbdb12e3b413febf7f888dbc43d3b59facf55ea21b9d38a54617643638b12" +checksum = "6386bc9cc6bd0095deac02a0218e27afaf398f2e2a298c482d8267c3cee0d65c" dependencies = [ + "bisync", "embedded-hal 1.0.0", + "embedded-hal-async", "smart-leds-trait", + "ux", ] [[package]] @@ -68,7 +71,7 @@ dependencies = [ "cortex-m-rt", "credentials", "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "defmt-test", "embassy-executor", @@ -77,7 +80,7 @@ dependencies = [ "embassy-sync", "embassy-time", "futures-util", - "heapless 0.7.17", + "heapless 0.9.1", "libm", "nalgebra", "panic-probe", @@ -90,28 +93,19 @@ name = "ateam-lib-stm32" version = "0.1.0" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "embassy-executor", "embassy-futures", "embassy-stm32", "embassy-sync", "embassy-time", - "heapless 0.8.0", + "heapless 0.9.1", "num-traits", "paste", "smart-leds", ] -[[package]] -name = "atomic-polyfill" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" -dependencies = [ - "critical-section", -] - [[package]] name = "atty" version = "0.2.14" @@ -125,9 +119,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bare-metal" @@ -135,7 +129,7 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" dependencies = [ - "rustc_version 0.2.3", + "rustc_version", ] [[package]] @@ -161,6 +155,21 @@ dependencies = [ "which", ] +[[package]] +name = "bisync" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5020822f6d6f23196ccaf55e228db36f9de1cf788052b37992e17cbc96ec41a7" +dependencies = [ + "bisync_macros", +] + +[[package]] +name = "bisync_macros" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d21f40d350a700f6aa107e45fb26448cf489d34794b2ba4522181dc9f1173af6" + [[package]] name = "bit_field" version = "0.10.2" @@ -181,9 +190,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.8.0" +version = "2.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36" +checksum = "34efbcccd345379ca2868b2b2c9d3782e9cc58ba87bc7d79d5b53d9c9ae6f25d" [[package]] name = "block-device-driver" @@ -196,9 +205,9 @@ dependencies = [ [[package]] name = "bytemuck" -version = "1.21.0" +version = "1.23.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" +checksum = "3995eaeebcdf32f91f980d360f78732ddc061097ab4e39991ae7a6ace9194677" [[package]] name = "byteorder" @@ -217,15 +226,15 @@ dependencies = [ [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9" [[package]] name = "chrono" -version = "0.4.39" +version = "0.4.41" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825" +checksum = "c469d952047f47f91b68d1cba3f10d63c11d73e4636f24f08daf0278abf01c4d" dependencies = [ "num-traits", ] @@ -339,9 +348,9 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "darling" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" dependencies = [ "darling_core", "darling_macro", @@ -349,9 +358,9 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", @@ -363,9 +372,9 @@ dependencies = [ [[package]] name = "darling_macro" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", "quote", @@ -374,9 +383,18 @@ dependencies = [ [[package]] name = "defmt" -version = "0.3.10" +version = "0.3.100" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -384,9 +402,9 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.4.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", @@ -397,40 +415,40 @@ dependencies = [ [[package]] name = "defmt-parser" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", ] [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "24076cc7203c365e7febfcec15d6667a9ef780bd2c5fd3b2a197400df78f299b" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", - "defmt", + "defmt 1.0.1", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", @@ -448,16 +466,16 @@ dependencies = [ [[package]] name = "either" -version = "1.13.0" +version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" +checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", "embassy-futures", "embassy-hal-internal", "embassy-sync", @@ -472,20 +490,20 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.8.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-executor-macros", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.7.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "darling", "proc-macro2", @@ -496,42 +514,42 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-hal-internal" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "num-traits", ] [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "aligned", "bit_field", - "bitflags 2.8.0", + "bitflags 2.9.3", "block-device-driver", "cfg-if", "chrono", "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-embedded-hal", "embassy-futures", @@ -556,7 +574,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.3", "sdio-host", "static_assertions", "stm32-fmc", @@ -567,46 +586,46 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.7.1" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", "heapless 0.8.0", ] [[package]] name = "embassy-time" version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-time-driver", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", - "futures-util", + "futures-core", ] [[package]] name = "embassy-time-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "embassy-executor", "heapless 0.8.0", @@ -614,19 +633,20 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "embassy-sync", "embassy-usb-driver", ] @@ -681,7 +701,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ - "defmt", + "defmt 0.3.100", ] [[package]] @@ -690,7 +710,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" dependencies = [ - "defmt", + "defmt 0.3.100", "embedded-io", ] @@ -724,12 +744,12 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.10" +version = "0.3.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "33d852cb9b869c2a9b3df2f71a3074817f01e1844f839a144f5fcef059a4eb5d" +checksum = "778e2ac28f6c47af28e4907f13ffd1e1ddbd400980a9abd7c8df189bf578a5ad" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -770,18 +790,9 @@ dependencies = [ [[package]] name = "glob" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2" - -[[package]] -name = "hash32" -version = "0.2.1" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" -dependencies = [ - "byteorder", -] +checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280" [[package]] name = "hash32" @@ -800,24 +811,21 @@ checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" [[package]] name = "heapless" -version = "0.7.17" +version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" dependencies = [ - "atomic-polyfill", - "hash32 0.2.1", - "rustc_version 0.4.1", - "spin", + "hash32", "stable_deref_trait", ] [[package]] name = "heapless" -version = "0.8.0" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5" dependencies = [ - "hash32 0.3.1", + "hash32", "stable_deref_trait", ] @@ -836,14 +844,14 @@ version = "0.5.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589533453244b0995c858700322199b2becb13b627df2851f64a2775d024abcf" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] name = "humantime" -version = "2.1.0" +version = "2.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" +checksum = "9b112acc8b3adf4b107a8ec20977da0273a8c386765a3ec0229bd500a1443f9f" [[package]] name = "ident_case" @@ -875,25 +883,25 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.169" +version = "0.2.175" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5aba8db14291edd000dfcc4d620c7ebfb122c613afb886ca8803fa4e128a20a" +checksum = "6a82ae493e598baaea5209805c49bbf2ea7de956d50d7da0da1164f9c6d28543" [[package]] name = "libloading" -version = "0.8.6" +version = "0.8.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc2f4eb4bc735547cfed7c0a4922cbd04a4655978c09b54f1f7b228750664c34" +checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667" dependencies = [ "cfg-if", - "windows-targets", + "windows-targets 0.53.3", ] [[package]] name = "libm" -version = "0.2.11" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" [[package]] name = "linux-raw-sys" @@ -903,31 +911,21 @@ checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" [[package]] name = "litrs" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" - -[[package]] -name = "lock_api" -version = "0.4.12" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" -dependencies = [ - "autocfg", - "scopeguard", -] +checksum = "f5e54036fe321fd421e10d732f155734c4e4afd610dd556d9a82833ab3ee0bed" [[package]] name = "log" -version = "0.4.25" +version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "04cbf5b083de1c7e0222a7a51dbfdba1cbe1c6ab0b15e29fff3f6c077fd9cd9f" +checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "memchr" -version = "2.7.4" +version = "2.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" +checksum = "32a282da65faaf38286cf3be983213fcf1d2e2a58700e808f83f4ea9a4804bc0" [[package]] name = "minimal-lexical" @@ -937,9 +935,9 @@ checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" [[package]] name = "nalgebra" -version = "0.33.2" +version = "0.34.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b" +checksum = "9cd59afb6639828b33677758314a4a1a745c15c02bc597095b851c8fd915cf49" dependencies = [ "approx", "nalgebra-macros", @@ -952,9 +950,9 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.2" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" dependencies = [ "proc-macro2", "quote", @@ -1026,9 +1024,9 @@ dependencies = [ [[package]] name = "once_cell" -version = "1.20.3" +version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "945462a4b81e43c4e3ba96bd7b49d834c6f61198356aa858733bc4acf3cbe62e" +checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" [[package]] name = "os_str_bytes" @@ -1038,12 +1036,12 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +checksum = "fd402d00b0fb94c5aee000029204a46884b1262e0c443f166d86d2c0747e1a1a" dependencies = [ "cortex-m", - "defmt", + "defmt 1.0.1", ] [[package]] @@ -1072,9 +1070,9 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" [[package]] name = "portable-atomic" -version = "1.10.0" +version = "1.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" +checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483" [[package]] name = "proc-macro-error-attr2" @@ -1100,18 +1098,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.93" +version = "1.0.101" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.38" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -1122,11 +1120,17 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38" + [[package]] name = "regex" -version = "1.11.1" +version = "1.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191" +checksum = "23d7fd106d8c02486a8d64e778353d1cffe08ce79ac2e82f540c86d0facf6912" dependencies = [ "aho-corasick", "memchr", @@ -1136,9 +1140,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.9" +version = "0.4.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908" +checksum = "6b9458fa0bfeeac22b5ca447c63aaf45f28439a709ccd244698632f9aa6394d6" dependencies = [ "aho-corasick", "memchr", @@ -1147,15 +1151,15 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.5" +version = "0.8.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c" +checksum = "caf4aa5b0f434c91fe5c7f1ecb6a5ece2130b02ad2a590589dda5146df959001" [[package]] name = "rgb" -version = "0.8.50" +version = "0.8.52" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57397d16646700483b67d2dd6511d79318f9d057fdbd21a4066aeac8b41d310a" +checksum = "0c6a884d2998352bb4daf0183589aec883f16a6da1f4dde84d8e2e9a5409a1ce" dependencies = [ "bytemuck", ] @@ -1172,16 +1176,7 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" dependencies = [ - "semver 0.9.0", -] - -[[package]] -name = "rustc_version" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" -dependencies = [ - "semver 1.0.25", + "semver", ] [[package]] @@ -1190,24 +1185,18 @@ version = "0.38.44" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fdb5bc1ae2baa591800df16c9ca78619bf65c0488b41b96ccec5d11220d8c154" dependencies = [ - "bitflags 2.8.0", + "bitflags 2.9.3", "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.59.0", ] -[[package]] -name = "scopeguard" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" - [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -1218,12 +1207,6 @@ dependencies = [ "semver-parser", ] -[[package]] -name = "semver" -version = "1.0.25" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f79dfe2d285b0488816f30e700a7438c5a73d816b5b7d3ac72fbc48b0d185e03" - [[package]] name = "semver-parser" version = "0.7.0" @@ -1266,15 +1249,6 @@ dependencies = [ "rgb", ] -[[package]] -name = "spin" -version = "0.9.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" -dependencies = [ - "lock_api", -] - [[package]] name = "stable_deref_trait" version = "1.2.0" @@ -1289,9 +1263,9 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" [[package]] name = "static_cell" -version = "2.1.0" +version = "2.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" +checksum = "0530892bb4fa575ee0da4b86f86c667132a94b74bb72160f58ee5a4afec74c23" dependencies = [ "portable-atomic", ] @@ -1307,12 +1281,12 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +version = "17.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] @@ -1329,9 +1303,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" -version = "2.0.98" +version = "2.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "36147f1a48ae0ec2b5b3bc5b537d267457555a10dc06f3dbc8cb11ba3006d3b1" +checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6" dependencies = [ "proc-macro2", "quote", @@ -1349,24 +1323,24 @@ dependencies = [ [[package]] name = "textwrap" -version = "0.16.1" +version = "0.16.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" +checksum = "c13547615a44dc9c452a8a534638acdf07120d4b6847c8178705da06306a3057" [[package]] name = "thiserror" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d452f284b73e6d76dd36758a0c8684b1d5be31f92b89d07fd5822175732206fc" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26afc1baea8a989337eeb52b6e72a039780ce45c3edfcc9c5b9d112feeb173c2" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -1375,15 +1349,15 @@ dependencies = [ [[package]] name = "typenum" -version = "1.17.0" +version = "1.18.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" +checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f" [[package]] name = "unicode-ident" -version = "1.0.16" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a210d160f08b701c8721ba1c726c11662f877ea6b7094007e1ca9a1041945034" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unicode-xid" @@ -1391,6 +1365,12 @@ version = "0.2.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" +[[package]] +name = "ux" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b59fc5417e036e53226bbebd90196825d358624fd5577432c4e486c95b1b096" + [[package]] name = "vcell" version = "0.1.3" @@ -1442,11 +1422,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.9" +version = "0.1.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +checksum = "0978bf7171b3d90bac376700cb56d606feb40f251a475a5d6634613564460b22" dependencies = [ - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -1455,13 +1435,28 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" +[[package]] +name = "windows-link" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a" + [[package]] name = "windows-sys" version = "0.59.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" dependencies = [ - "windows-targets", + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2f500e4d28234f72040990ec9d39e3a6b950f9f22d3dba18416c35882612bcb" +dependencies = [ + "windows-targets 0.53.3", ] [[package]] @@ -1470,14 +1465,31 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm 0.52.6", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", +] + +[[package]] +name = "windows-targets" +version = "0.53.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5fe6031c4041849d7c496a8ded650796e7b6ecc19df1a431c1a363342e5dc91" +dependencies = [ + "windows-link", + "windows_aarch64_gnullvm 0.53.0", + "windows_aarch64_msvc 0.53.0", + "windows_i686_gnu 0.53.0", + "windows_i686_gnullvm 0.53.0", + "windows_i686_msvc 0.53.0", + "windows_x86_64_gnu 0.53.0", + "windows_x86_64_gnullvm 0.53.0", + "windows_x86_64_msvc 0.53.0", ] [[package]] @@ -1486,44 +1498,92 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764" + [[package]] name = "windows_aarch64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" +[[package]] +name = "windows_aarch64_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c" + [[package]] name = "windows_i686_gnu" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" +[[package]] +name = "windows_i686_gnu" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3" + [[package]] name = "windows_i686_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" +[[package]] +name = "windows_i686_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11" + [[package]] name = "windows_i686_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" +[[package]] +name = "windows_i686_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d" + [[package]] name = "windows_x86_64_gnu" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" +[[package]] +name = "windows_x86_64_gnu" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba" + [[package]] name = "windows_x86_64_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57" + [[package]] name = "windows_x86_64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index c03c8777..cc34d27e 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -9,22 +9,21 @@ edition = "2021" cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -defmt = "0.3" -defmt-rtt = "0.4.1" +defmt = "1.0.1" +defmt-rtt = "1.0.0" -embassy-executor = { version = "0.7.0", features = [ +embassy-executor = { version = "0.8.0", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", - "task-arena-size-32768", ] } embassy-time = { version = "0.4.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.2.0", features = [ +embassy-stm32 = { version = "0.3.0", features = [ "defmt", "stm32h723zg", "unstable-pac", @@ -33,22 +32,22 @@ embassy-stm32 = { version = "0.2.0", features = [ "chrono" ] } embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.6.2" } +embassy-sync = { version = "0.7.1" } -static_cell = "2.1.0" +static_cell = "2.1.1" futures-util = { version = "0.3.31", default-features = false } -panic-probe = { version = "0.3", features = ["print-defmt"] } +panic-probe = { version = "1.0.0", features = ["print-defmt"] } critical-section = "1.1.1" -heapless = "0.7.16" +heapless = "0.9.1" const_format = "0.2.34" -libm = "0.2.6" -nalgebra = { version = "0.33.2", default-features = false, features = [ +libm = "0.2.15" +nalgebra = { version = "0.34.0", default-features = false, features = [ "libm", "macros", ] } smart-leds = "0.4.0" -apa102-spi = "0.4.0" +apa102-spi = "0.5.1" ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } @@ -59,7 +58,7 @@ default = [] no-private-credentials = ["credentials/no-private-credentials"] [dev-dependencies] -defmt-test = "0.3.0" +defmt-test = "0.4.0" [profile.dev] opt-level = 3 diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index 9cd06f65..2cf82d96 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -72,8 +72,8 @@ async fn main(_spawner: embassy_executor::Spawner) { &KICKER_IDLE_BUFFERED_UART, KICKER_IDLE_BUFFERED_UART.get_uart_read_queue(), KICKER_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PA8, - p.PA9, + p.PA8.into(), + p.PA9.into(), Pull::Up, true, ); diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index 4d06e9bf..98aff815 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -69,8 +69,8 @@ async fn main(_spawner: embassy_executor::Spawner) { &KICKER_IDLE_BUFFERED_UART, KICKER_IDLE_BUFFERED_UART.get_uart_read_queue(), KICKER_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PG2, - p.PG3, + p.PG2.into(), + p.PG3.into(), Pull::Up, true, ); diff --git a/control-board/src/bin/hwtest-piezo/main.rs b/control-board/src/bin/hwtest-piezo/main.rs index c2fe2402..5a6be5ae 100644 --- a/control-board/src/bin/hwtest-piezo/main.rs +++ b/control-board/src/bin/hwtest-piezo/main.rs @@ -58,7 +58,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_io_task!(main_spawner, robot_state, p); - let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); + let ch2 = PwmPin::new(p.PE6, OutputType::PushPull); let pwm = SimplePwm::new( p.TIM15, None, diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index d73ea8c4..984f804a 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -3,8 +3,8 @@ use ateam_lib_stm32::{ uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, }; use embassy_stm32::{ - gpio::{Pin, Pull}, - usart::Parity, + gpio::{AnyPin, Pull}, + usart::Parity, Peri, }; use embassy_time::{with_timeout, Duration, Timer}; @@ -63,8 +63,8 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: impl Pin, - reset_pin: impl Pin, + boot0_pin: Peri<'static, AnyPin>, + reset_pin: Peri<'static, AnyPin>, firmware_image: &'a [u8], ) -> Self { let stm32_interface = Stm32Interface::new_from_pins( diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 1d5853f9..2d0f7bdb 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -12,8 +12,8 @@ use core::fmt::Write; use core::mem::size_of; use credentials::WifiCredential; use embassy_futures::select::{select, Either}; -use embassy_stm32::gpio::{Level, Output, Pin, Speed}; -use embassy_stm32::uid; +use embassy_stm32::gpio::{AnyPin, Level, Output, Speed}; +use embassy_stm32::{uid, Peri}; use embassy_stm32::usart::{self, DataBits, Parity, StopBits}; use embassy_time::{Duration, Timer}; use heapless::String; @@ -112,7 +112,7 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - reset_pin: impl Pin, + reset_pin: Peri<'static, AnyPin>, use_flow_control: bool, ) -> RobotRadio<'a, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX, DEBUG_UART_QUEUES> { let reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index 27d35c08..de277c97 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -1,4 +1,4 @@ -use embassy_stm32::gpio::{Level, Output, Pin, Speed}; +use embassy_stm32::{gpio::{AnyPin, Level, Output, Speed}, Peri}; pub struct ShellIndicator<'a> { fr_pin0: Output<'a>, @@ -20,11 +20,11 @@ impl<'a> ShellIndicator<'a> { // TODO: refactor pin ordering pub fn new( - fr_pin0: impl Pin, - fl_pin1: impl Pin, - br_pin2: impl Pin, - bl_pin3: impl Pin, - team_pin4: Option, + fr_pin0: Peri<'static, AnyPin>, + fl_pin1: Peri<'static, AnyPin>, + br_pin2: Peri<'static, AnyPin>, + bl_pin3: Peri<'static, AnyPin>, + team_pin4: Option>, ) -> Self { let team_pin4: Option> = if let Some(pin4) = team_pin4 { Some(Output::new(pin4, Level::Low, Speed::Low)) diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 031364c2..b1299d32 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -4,9 +4,7 @@ #![feature(inherent_associated_types)] #![feature(generic_const_exprs)] #![feature(type_alias_impl_trait)] -#![feature(async_closure)] #![feature( - maybe_uninit_uninit_array, maybe_uninit_slice, maybe_uninit_write_slice )] diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index f55c4f5a..fa400cb8 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -4,7 +4,7 @@ use ateam_common_packets::{ bindings::{KickerTelemetry, PowerTelemetry}, radio::{DataPacket, TelemetryPacket}, }; -use embassy_stm32::{dma::NoDma, peripherals::*}; +use embassy_stm32::{mode::Blocking, peripherals::*}; use embassy_sync::{ blocking_mutex::raw::ThreadModeRawMutex, pubsub::{PubSubChannel, Publisher, Subscriber}, @@ -230,8 +230,8 @@ pub type OpticalFlowUartRxPin = PB8; pub type OpticalFlowUartTxPin = PB9; pub type OpticalFlowUartCtsPin = PB15; pub type OpticalFlowUartRtsPin = PB14; -pub type OpticalFlowDmaRx = NoDma; -pub type OpticalFlowDmaTx = NoDma; +pub type OpticalFlowDmaRx = Blocking; +pub type OpticalFlowDmaTx = Blocking; pub type OpticalFlowBootPin = PB7; pub type OpticalFlowResetPin = PB6; @@ -242,8 +242,8 @@ pub type OpticalFlowResetPin = PB6; pub type ScreenUart = USART1; pub type ScreenUartRxPin = PA10; pub type ScreenUartTxPin = PA9; -pub type ScreenDmaRx = NoDma; -pub type ScreenDmaTx = NoDma; +pub type ScreenDmaRx = Blocking; +pub type ScreenDmaTx = Blocking; pub type ScreenResetPin = PA8; /////////// @@ -288,8 +288,8 @@ pub type ExtImuNDetPin = PB2; pub type ShellDetectI2c = I2C2; pub type ShellDetectSdaPin = PF0; pub type ShellDetectSclPin = PF1; -pub type ShellDetectDmaRx = NoDma; -pub type ShellDetectDmaTx = NoDma; +pub type ShellDetectDmaRx = Blocking; +pub type ShellDetectDmaTx = Blocking; pub type BatteryAdcPin = PA0; pub type VoltageMon5v0Pin = PA1; diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 30451071..5409a392 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -6,8 +6,8 @@ use ateam_lib_stm32::{ }; use defmt::*; use embassy_stm32::{ - gpio::{Pin, Pull}, - usart::Parity, + gpio::{AnyPin, Pull}, + usart::Parity, Peri, }; use embassy_time::{with_timeout, Duration, Timer}; use nalgebra::Vector3; @@ -99,8 +99,8 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: impl Pin, - reset_pin: impl Pin, + boot0_pin: Peri<'static, AnyPin>, + reset_pin: Peri<'static, AnyPin>, firmware_image: &'a [u8], ) -> WheelMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // Need a Pull None to allow for STSPIN watchdog usage. diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index c158a069..11ef7fc8 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -6,7 +6,7 @@ use embassy_stm32::{ timer::{ simple_pwm::{PwmPin, SimplePwm}, Channel, - }, + }, Peri, }; use embassy_time::{Duration, Ticker}; @@ -60,10 +60,10 @@ async fn audio_task_entry( pub fn start_audio_task( task_spawner: &Spawner, robot_state: &'static SharedRobotState, - buzzer_timer: BuzzerTimer, - buzzer_pin: BuzzerPin, + buzzer_timer: Peri<'static, BuzzerTimer>, + buzzer_pin: Peri<'static, BuzzerPin>, ) { - let ch2 = PwmPin::new_ch2(buzzer_pin, OutputType::PushPull); + let ch2 = PwmPin::new(buzzer_pin, OutputType::PushPull); let pwm = SimplePwm::new( buzzer_timer, None, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 1798ec5d..50d4f90c 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -6,7 +6,7 @@ use ateam_lib_stm32::{ drivers::boot::stm32_interface, idle_buffered_uart_spawn_tasks, static_idle_buffered_uart, }; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::usart::Uart; +use embassy_stm32::{usart::Uart, Peri}; use embassy_time::{Duration, Instant, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; @@ -632,34 +632,34 @@ pub async fn start_control_task( accel_subscriber: AccelDataSubscriber, power_telemetry_subscriber: PowerTelemetrySubscriber, kicker_telemetry_subscriber: KickerTelemetrySubscriber, - motor_fl_uart: MotorFLUart, - motor_fl_rx_pin: MotorFLUartRxPin, - motor_fl_tx_pin: MotorFLUartTxPin, - motor_fl_rx_dma: MotorFLDmaRx, - motor_fl_tx_dma: MotorFLDmaTx, - motor_fl_boot0_pin: MotorFLBootPin, - motor_fl_nrst_pin: MotorFLResetPin, - motor_bl_uart: MotorBLUart, - motor_bl_rx_pin: MotorBLUartRxPin, - motor_bl_tx_pin: MotorBLUartTxPin, - motor_bl_rx_dma: MotorBLDmaRx, - motor_bl_tx_dma: MotorBLDmaTx, - motor_bl_boot0_pin: MotorBLBootPin, - motor_bl_nrst_pin: MotorBLResetPin, - motor_br_uart: MotorBRUart, - motor_br_rx_pin: MotorBRUartRxPin, - motor_br_tx_pin: MotorBRUartTxPin, - motor_br_rx_dma: MotorBRDmaRx, - motor_br_tx_dma: MotorBRDmaTx, - motor_br_boot0_pin: MotorBRBootPin, - motor_br_nrst_pin: MotorBRResetPin, - motor_fr_uart: MotorFRUart, - motor_fr_rx_pin: MotorFRUartRxPin, - motor_fr_tx_pin: MotorFRUartTxPin, - motor_fr_rx_dma: MotorFRDmaRx, - motor_fr_tx_dma: MotorFRDmaTx, - motor_fr_boot0_pin: MotorFRBootPin, - motor_fr_nrst_pin: MotorFRResetPin, + motor_fl_uart: Peri<'static, MotorFLUart>, + motor_fl_rx_pin: Peri<'static, MotorFLUartRxPin>, + motor_fl_tx_pin: Peri<'static, MotorFLUartTxPin>, + motor_fl_rx_dma: Peri<'static, MotorFLDmaRx>, + motor_fl_tx_dma: Peri<'static, MotorFLDmaTx>, + motor_fl_boot0_pin: Peri<'static, MotorFLBootPin>, + motor_fl_nrst_pin: Peri<'static, MotorFLResetPin>, + motor_bl_uart: Peri<'static, MotorBLUart>, + motor_bl_rx_pin: Peri<'static, MotorBLUartRxPin>, + motor_bl_tx_pin: Peri<'static, MotorBLUartTxPin>, + motor_bl_rx_dma: Peri<'static, MotorBLDmaRx>, + motor_bl_tx_dma: Peri<'static, MotorBLDmaTx>, + motor_bl_boot0_pin: Peri<'static, MotorBLBootPin>, + motor_bl_nrst_pin: Peri<'static, MotorBLResetPin>, + motor_br_uart: Peri<'static, MotorBRUart>, + motor_br_rx_pin: Peri<'static, MotorBRUartRxPin>, + motor_br_tx_pin: Peri<'static, MotorBRUartTxPin>, + motor_br_rx_dma: Peri<'static, MotorBRDmaRx>, + motor_br_tx_dma: Peri<'static, MotorBRDmaTx>, + motor_br_boot0_pin: Peri<'static, MotorBRBootPin>, + motor_br_nrst_pin: Peri<'static, MotorBRResetPin>, + motor_fr_uart: Peri<'static, MotorFRUart>, + motor_fr_rx_pin: Peri<'static, MotorFRUartRxPin>, + motor_fr_tx_pin: Peri<'static, MotorFRUartTxPin>, + motor_fr_rx_dma: Peri<'static, MotorFRDmaRx>, + motor_fr_tx_dma: Peri<'static, MotorFRDmaTx>, + motor_fr_boot0_pin: Peri<'static, MotorFRBootPin>, + motor_fr_nrst_pin: Peri<'static, MotorFRResetPin>, ) { let initial_motor_controller_uart_conifg = stm32_interface::get_bootloader_uart_config(); @@ -730,32 +730,32 @@ pub async fn start_control_task( &FRONT_LEFT_IDLE_BUFFERED_UART, FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), - motor_fl_boot0_pin, - motor_fl_nrst_pin, + motor_fl_boot0_pin.into(), + motor_fl_nrst_pin.into(), WHEEL_FW_IMG, ); let motor_bl = WheelMotor::new_from_pins( &BACK_LEFT_IDLE_BUFFERED_UART, BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), - motor_bl_boot0_pin, - motor_bl_nrst_pin, + motor_bl_boot0_pin.into(), + motor_bl_nrst_pin.into(), WHEEL_FW_IMG, ); let motor_br = WheelMotor::new_from_pins( &BACK_RIGHT_IDLE_BUFFERED_UART, BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), - motor_br_boot0_pin, - motor_br_nrst_pin, + motor_br_boot0_pin.into(), + motor_br_nrst_pin.into(), WHEEL_FW_IMG, ); let motor_fr = WheelMotor::new_from_pins( &FRONT_RIGHT_IDLE_BUFFERED_UART, FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), - motor_fr_boot0_pin, - motor_fr_nrst_pin, + motor_fr_boot0_pin.into(), + motor_fr_nrst_pin.into(), WHEEL_FW_IMG, ); diff --git a/control-board/src/tasks/dotstar_task.rs b/control-board/src/tasks/dotstar_task.rs index b29f685c..e48771f8 100644 --- a/control-board/src/tasks/dotstar_task.rs +++ b/control-board/src/tasks/dotstar_task.rs @@ -1,6 +1,7 @@ use ateam_lib_stm32::drivers::led::apa102::{apa102_buf_len, Apa102}; use embassy_executor::Spawner; +use embassy_stm32::Peri; use smart_leds::colors::{BLACK, BLUE, CYAN, GREEN, ORANGE_RED, PURPLE, RED, VIOLET, YELLOW}; use crate::{pins::*, MotorIndex}; @@ -209,10 +210,10 @@ static mut DOTSTAR_SPI_BUFFER_CELL: Apa102Buf = [0; LED_BUF_LEN]; pub async fn start_dotstar_task( spawner: &Spawner, led_command_subscriber: LedCommandSubscriber, - dotstar_peri: DotstarSpi, - dotstar_sck_pin: DotstarSpiSck, - dotstar_mosi_pin: DotstarSpiMosi, - dotstar_tx_dma: DotstarTxDma, + dotstar_peri: Peri<'static, DotstarSpi>, + dotstar_sck_pin: Peri<'static, DotstarSpiSck>, + dotstar_mosi_pin: Peri<'static, DotstarSpiMosi>, + dotstar_tx_dma: Peri<'static, DotstarTxDma>, ) { let dotstar_spi_buf: &'static mut Apa102Buf = unsafe { &mut *(&raw mut DOTSTAR_SPI_BUFFER_CELL) }; diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index fbd9adb0..55a4fbb7 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -3,8 +3,8 @@ use embassy_futures::select::{select, Either}; use embassy_stm32::exti::ExtiInput; use embassy_stm32::gpio::Pull; use embassy_stm32::spi::{MisoPin, MosiPin, SckPin}; -use embassy_stm32::Peripheral; +use embassy_stm32::Peri; use embassy_time::{Instant, Timer}; use nalgebra::Vector3; @@ -222,20 +222,20 @@ pub fn start_imu_task( gyro_data_publisher: GyroDataPublisher, accel_data_publisher: AccelDataPublisher, led_cmd_publisher: LedCommandPublisher, - peri: impl Peripheral

+ 'static, - sck: impl Peripheral

> + 'static, - mosi: impl Peripheral

> + 'static, - miso: impl Peripheral

> + 'static, - txdma: impl Peripheral

+ 'static, - rxdma: impl Peripheral

+ 'static, - bmi323_nss: ImuSpiNss0Pin, - _ext_nss1_pin: ExtImuSpiNss1Pin, - _ext_nss2_pin: ExtImuSpiNss2Pin, - accel_int_pin: impl Peripheral

+ 'static, - gyro_int_pin: impl Peripheral

+ 'static, - accel_int: impl Peripheral

::ExtiChannel> + 'static, - gyro_int: impl Peripheral

::ExtiChannel> + 'static, - _ext_imu_det_pin: ExtImuNDetPin, + peri: Peri<'static, ImuSpi>, + sck: Peri<'static, impl SckPin>, + mosi: Peri<'static, impl MosiPin>, + miso: Peri<'static, impl MisoPin>, + txdma: Peri<'static, ImuSpiTxDma>, + rxdma: Peri<'static, ImuSpiRxDma>, + bmi323_nss: Peri<'static, ImuSpiNss0Pin>, + _ext_nss1_pin: Peri<'static, ExtImuSpiNss1Pin>, + _ext_nss2_pin: Peri<'static, ExtImuSpiNss2Pin>, + accel_int_pin: Peri<'static, ImuSpiInt1Pin>, + gyro_int_pin: Peri<'static, ImuSpiInt2Pin>, + accel_int: Peri<'static, ::ExtiChannel>, + gyro_int: Peri<'static, ::ExtiChannel>, + _ext_imu_det_pin: Peri<'static, ExtImuNDetPin>, ) { defmt::debug!("starting imu task..."); @@ -243,7 +243,7 @@ pub fn start_imu_task( // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; - let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); + let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss.into(), imu_buf); // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain @@ -269,20 +269,20 @@ pub fn start_imu_task_via_ie( gyro_data_publisher: GyroDataPublisher, accel_data_publisher: AccelDataPublisher, led_cmd_publisher: LedCommandPublisher, - peri: impl Peripheral

+ 'static, - sck: impl Peripheral

> + 'static, - mosi: impl Peripheral

> + 'static, - miso: impl Peripheral

> + 'static, - txdma: impl Peripheral

+ 'static, - rxdma: impl Peripheral

+ 'static, - bmi323_nss: ImuSpiNss0Pin, - _ext_nss1_pin: ExtImuSpiNss1Pin, - _ext_nss2_pin: ExtImuSpiNss2Pin, - accel_int_pin: impl Peripheral

+ 'static, - gyro_int_pin: impl Peripheral

+ 'static, - accel_int: impl Peripheral

::ExtiChannel> + 'static, - gyro_int: impl Peripheral

::ExtiChannel> + 'static, - _ext_imu_det_pin: ExtImuNDetPin, + peri: Peri<'static, ImuSpi>, + sck: Peri<'static, impl SckPin>, + mosi: Peri<'static, impl MosiPin>, + miso: Peri<'static, impl MisoPin>, + txdma: Peri<'static, ImuSpiTxDma>, + rxdma: Peri<'static, ImuSpiRxDma>, + bmi323_nss: Peri<'static, ImuSpiNss0Pin>, + _ext_nss1_pin: Peri<'static, ExtImuSpiNss1Pin>, + _ext_nss2_pin: Peri<'static, ExtImuSpiNss2Pin>, + accel_int_pin: Peri<'static, ImuSpiInt1Pin>, + gyro_int_pin: Peri<'static, ImuSpiInt2Pin>, + accel_int: Peri<'static, ::ExtiChannel>, + gyro_int: Peri<'static, ::ExtiChannel>, + _ext_imu_det_pin: Peri<'static, ExtImuNDetPin>, ) { defmt::debug!("starting imu task..."); @@ -290,7 +290,7 @@ pub fn start_imu_task_via_ie( // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; - let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); + let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss.into(), imu_buf); // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index ea9f495c..757eaa5c 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -5,7 +5,7 @@ use ateam_lib_stm32::{ uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, }; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::{gpio::Pin, usart::Uart}; +use embassy_stm32::{gpio::AnyPin, usart::Uart, Peri}; use embassy_sync::pubsub::WaitResult; use embassy_time::{Duration, Instant, Ticker}; @@ -101,8 +101,8 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: impl Pin, - reset_pin: impl Pin, + boot0_pin: Peri<'static, AnyPin>, + reset_pin: Peri<'static, AnyPin>, firmware_image: &'a [u8], robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, @@ -352,13 +352,13 @@ pub async fn start_kicker_task( robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, kicker_telemetry_publisher: KickerTelemetryPublisher, - kicker_uart: KickerUart, - kicker_uart_rx_pin: KickerUartRxPin, - kicker_uart_tx_pin: KickerUartTxPin, - kicker_uart_rx_dma: KickerRxDma, - kicker_uart_tx_dma: KickerTxDma, - kicker_boot0_pin: KickerBootPin, - kicker_reset_pin: KickerResetPin, + kicker_uart: Peri<'static, KickerUart>, + kicker_uart_rx_pin: Peri<'static, KickerUartRxPin>, + kicker_uart_tx_pin: Peri<'static, KickerUartTxPin>, + kicker_uart_rx_dma: Peri<'static, KickerRxDma>, + kicker_uart_tx_dma: Peri<'static, KickerTxDma>, + kicker_boot0_pin: Peri<'static, KickerBootPin>, + kicker_reset_pin: Peri<'static, KickerResetPin>, ) { let initial_kicker_uart_conifg = stm32_interface::get_bootloader_uart_config(); @@ -380,8 +380,8 @@ pub async fn start_kicker_task( &KICKER_IDLE_BUFFERED_UART, KICKER_IDLE_BUFFERED_UART.get_uart_read_queue(), KICKER_IDLE_BUFFERED_UART.get_uart_write_queue(), - kicker_boot0_pin, - kicker_reset_pin, + kicker_boot0_pin.into(), + kicker_reset_pin.into(), KICKER_FW_IMG, robot_state, command_subscriber, diff --git a/control-board/src/tasks/power_task.rs b/control-board/src/tasks/power_task.rs index 7e49ff5f..08dac759 100644 --- a/control-board/src/tasks/power_task.rs +++ b/control-board/src/tasks/power_task.rs @@ -6,7 +6,7 @@ use ateam_lib_stm32::{ uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, }; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::usart::{self, DataBits, Parity, StopBits, Uart}; +use embassy_stm32::{usart::{self, DataBits, Parity, StopBits, Uart}, Peri}; use embassy_time::{Duration, Instant, Ticker, Timer}; use crate::{ @@ -241,11 +241,11 @@ pub fn start_power_task( robot_state: &'static SharedRobotState, power_telemetry_publisher: PowerTelemetryPublisher, led_command_publisher: LedCommandPublisher, - power_uart: PowerUart, - power_uart_rx_pin: PowerUartRxPin, - power_uart_tx_pin: PowerUartTxPin, - power_uart_rx_dma: PowerRxDma, - power_uart_tx_dma: PowerTxDma, + power_uart: Peri<'static, PowerUart>, + power_uart_rx_pin: Peri<'static, PowerUartRxPin>, + power_uart_tx_pin: Peri<'static, PowerUartTxPin>, + power_uart_rx_dma: Peri<'static, PowerRxDma>, + power_uart_tx_dma: Peri<'static, PowerTxDma>, ) { let uart_config = power_uart_config(); let power_uart = Uart::new( diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 8ed2573f..3366817a 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -7,8 +7,8 @@ use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ - gpio::{Input, Pin, Pull}, - usart::{self, DataBits, Parity, StopBits, Uart}, + gpio::{AnyPin, Input, Pull}, + usart::{self, DataBits, Parity, StopBits, Uart}, Peri, }; use embassy_time::{Duration, Instant, Ticker, Timer}; @@ -171,8 +171,8 @@ impl< RADIO_TX_BUF_DEPTH, DEBUG_UART_QUEUES, >, - radio_reset_pin: impl Pin, - radio_ndet_pin: impl Pin, + radio_reset_pin: Peri<'static, AnyPin>, + radio_ndet_pin: Peri<'static, AnyPin>, wifi_credentials: &'static [WifiCredential], ) -> Self { let radio = RobotRadio::new( @@ -655,15 +655,15 @@ pub async fn start_radio_task( telemetry_subscriber: TelemetrySubcriber, led_command_pub: LedCommandPublisher, wifi_credentials: &'static [WifiCredential], - radio_uart: RadioUART, - radio_uart_rx_pin: RadioUartRxPin, - radio_uart_tx_pin: RadioUartTxPin, - _radio_uart_cts_pin: RadioUartCtsPin, - _radio_uart_rts_pin: RadioUartRtsPin, - radio_uart_rx_dma: RadioRxDMA, - radio_uart_tx_dma: RadioTxDMA, - radio_reset_pin: RadioResetPin, - radio_ndet_pin: RadioNDetectPin, + radio_uart: Peri<'static, RadioUART>, + radio_uart_rx_pin: Peri<'static, RadioUartRxPin>, + radio_uart_tx_pin: Peri<'static, RadioUartTxPin>, + _radio_uart_cts_pin: Peri<'static, RadioUartCtsPin>, + _radio_uart_rts_pin: Peri<'static, RadioUartRtsPin>, + radio_uart_rx_dma: Peri<'static, RadioRxDMA>, + radio_uart_tx_dma: Peri<'static, RadioTxDMA>, + radio_reset_pin: Peri<'static, RadioResetPin>, + radio_ndet_pin: Peri<'static, RadioNDetectPin>, ) { let uart_conifg = startup_uart_config(); @@ -728,8 +728,8 @@ pub async fn start_radio_task( &RADIO_IDLE_BUFFERED_UART, RADIO_IDLE_BUFFERED_UART.get_uart_read_queue(), RADIO_IDLE_BUFFERED_UART.get_uart_write_queue(), - radio_reset_pin, - radio_ndet_pin, + radio_reset_pin.into(), + radio_ndet_pin.into(), wifi_credentials, ); diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index ce6b3e39..bd1d6c0c 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -2,6 +2,7 @@ use ateam_lib_stm32::filter::{Filter, IirFilter}; use ateam_lib_stm32::time::SyncTicker; use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; +use embassy_stm32::Peri; use embassy_time::{Duration, Ticker}; use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; @@ -66,7 +67,7 @@ macro_rules! create_io_task { #[embassy_executor::task] async fn user_io_task_entry( robot_state: &'static SharedRobotState, - mut battery_volt_adc: AdcHelper<'static, BatteryAdc, BatteryAdcPin>, + mut battery_volt_adc: AdcHelper<'static, BatteryAdc, Peri<'static, BatteryAdcPin>>, vref_int_adc: Adc<'static, VrefIntAdc>, dip_switch: DipSwitch<'static, 8>, debug_mode_dip_switch: DipSwitch<'static, 1>, @@ -245,36 +246,36 @@ async fn user_io_task_entry( pub async fn start_io_task( spawner: &Spawner, robot_state: &'static SharedRobotState, - battery_adc_peri: BatteryAdc, - battery_adc_pin: BatteryAdcPin, - vref_int_adc_peri: VrefIntAdc, - usr_dip7_pin: UsrDip7Pin, - usr_dip6_pin: UsrDip6Pin, - usr_dip5_pin: UsrDip5Pin, - usr_dip4_pin: UsrDip4Pin, - usr_dip3_pin: UsrDip3Pin, - usr_dip2_pin: UsrDip2Pin, - usr_dip1_pin: UsrDip1Pin, - usr_dip0_pin: UsrDip0Pin, - debug_mode_pin: UsrDipDebugMode, - robot_id_team_is_blue_pin: UsrDipTeamIsBluePin, - robot_id_src_select_pin: UsrDipBotIdSrcSelect, - robot_id_selector3_pin: RobotIdSelector3Pin, - robot_id_selector2_pin: RobotIdSelector2Pin, - robot_id_selector1_pin: RobotIdSelector1Pin, - robot_id_selector0_pin: RobotIdSelector0Pin, - usr_led0_pin: UsrLed0Pin, - usr_led1_pin: UsrLed1Pin, - usr_led2_pin: UsrLed2Pin, - usr_led3_pin: UsrLed3Pin, - robot_id_src_disagree_led_pin: RobotIdSrcDisagree, - robot_id_indicator_fl: RobotIdIndicator0FlPin, - robot_id_indicator_bl: RobotIdIndicator1BlPin, - robot_id_indicator_br: RobotIdIndicator2BrPin, - robot_id_indicator_fr: RobotIdIndicator3FrPin, - robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, + battery_adc_peri: Peri<'static, BatteryAdc>, + battery_adc_pin: Peri<'static, BatteryAdcPin>, + vref_int_adc_peri: Peri<'static, VrefIntAdc>, + usr_dip7_pin: Peri<'static, UsrDip7Pin>, + usr_dip6_pin: Peri<'static, UsrDip6Pin>, + usr_dip5_pin: Peri<'static, UsrDip5Pin>, + usr_dip4_pin: Peri<'static, UsrDip4Pin>, + usr_dip3_pin: Peri<'static, UsrDip3Pin>, + usr_dip2_pin: Peri<'static, UsrDip2Pin>, + usr_dip1_pin: Peri<'static, UsrDip1Pin>, + usr_dip0_pin: Peri<'static, UsrDip0Pin>, + debug_mode_pin: Peri<'static, UsrDipDebugMode>, + robot_id_team_is_blue_pin: Peri<'static, UsrDipTeamIsBluePin>, + robot_id_src_select_pin: Peri<'static, UsrDipBotIdSrcSelect>, + robot_id_selector3_pin: Peri<'static, RobotIdSelector3Pin>, + robot_id_selector2_pin: Peri<'static, RobotIdSelector2Pin>, + robot_id_selector1_pin: Peri<'static, RobotIdSelector1Pin>, + robot_id_selector0_pin: Peri<'static, RobotIdSelector0Pin>, + usr_led0_pin: Peri<'static, UsrLed0Pin>, + usr_led1_pin: Peri<'static, UsrLed1Pin>, + usr_led2_pin: Peri<'static, UsrLed2Pin>, + usr_led3_pin: Peri<'static, UsrLed3Pin>, + robot_id_src_disagree_led_pin: Peri<'static, RobotIdSrcDisagree>, + robot_id_indicator_fl: Peri<'static, RobotIdIndicator0FlPin>, + robot_id_indicator_bl: Peri<'static, RobotIdIndicator1BlPin>, + robot_id_indicator_br: Peri<'static, RobotIdIndicator2BrPin>, + robot_id_indicator_fr: Peri<'static, RobotIdIndicator3FrPin>, + robot_id_indicator_isblue: Peri<'static, RobotIdIndicator4TeamIsBluePin>, ) { - let dip_sw_pins: [AnyPin; 8] = [ + let dip_sw_pins: [Peri<'static, AnyPin>; 8] = [ usr_dip7_pin.into(), usr_dip6_pin.into(), usr_dip5_pin.into(), @@ -286,16 +287,16 @@ pub async fn start_io_task( ]; let dip_switch = DipSwitch::new_from_pins(dip_sw_pins, Pull::None, None); - let debug_mode_pins: [AnyPin; 1] = [debug_mode_pin.into()]; + let debug_mode_pins: [Peri<'static, AnyPin>; 1] = [debug_mode_pin.into()]; let debug_mode_dip = DipSwitch::new_from_pins(debug_mode_pins, Pull::None, Some([true])); - let team_color_pins: [AnyPin; 2] = [ + let team_color_pins: [Peri<'static, AnyPin>; 2] = [ robot_id_src_select_pin.into(), robot_id_team_is_blue_pin.into(), ]; let team_color_dip = DipSwitch::new_from_pins(team_color_pins, Pull::None, Some([false, true])); - let robot_id_selector_pins: [AnyPin; 4] = [ + let robot_id_selector_pins: [Peri<'static, AnyPin>; 4] = [ robot_id_selector3_pin.into(), robot_id_selector2_pin.into(), robot_id_selector1_pin.into(), @@ -311,11 +312,11 @@ pub async fn start_io_task( let robot_id_src_disagree_led = Output::new(robot_id_src_disagree_led_pin, Level::Low, Speed::Low); let robot_id_indicator = ShellIndicator::new( - robot_id_indicator_fr, - robot_id_indicator_fl, - robot_id_indicator_bl, - robot_id_indicator_br, - Some(robot_id_indicator_isblue), + robot_id_indicator_fr.into(), + robot_id_indicator_fl.into(), + robot_id_indicator_bl.into(), + robot_id_indicator_br.into(), + Some(robot_id_indicator_isblue.into()), ); let battery_volt_adc = AdcHelper::new( diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 0a8fce48..a0c0a08d 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -56,7 +56,7 @@ dependencies = [ "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "defmt-test", "embassy-executor", @@ -65,7 +65,7 @@ dependencies = [ "embassy-sync", "embassy-time", "futures-util", - "heapless 0.7.17", + "heapless 0.9.1", "libm", "nalgebra", "panic-probe", @@ -78,28 +78,19 @@ name = "ateam-lib-stm32" version = "0.1.0" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "embassy-executor", "embassy-futures", "embassy-stm32", "embassy-sync", "embassy-time", - "heapless 0.8.0", + "heapless 0.9.1", "num-traits", "paste", "smart-leds", ] -[[package]] -name = "atomic-polyfill" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" -dependencies = [ - "critical-section", -] - [[package]] name = "atty" version = "0.2.14" @@ -113,9 +104,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.3.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bare-metal" @@ -123,7 +114,7 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" dependencies = [ - "rustc_version 0.2.3", + "rustc_version", ] [[package]] @@ -169,9 +160,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.5.0" +version = "2.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" +checksum = "34efbcccd345379ca2868b2b2c9d3782e9cc58ba87bc7d79d5b53d9c9ae6f25d" [[package]] name = "block-device-driver" @@ -184,9 +175,9 @@ dependencies = [ [[package]] name = "bytemuck" -version = "1.16.0" +version = "1.23.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" +checksum = "3995eaeebcdf32f91f980d360f78732ddc061097ab4e39991ae7a6ace9194677" [[package]] name = "byteorder" @@ -205,15 +196,15 @@ dependencies = [ [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9" [[package]] name = "chrono" -version = "0.4.38" +version = "0.4.41" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" +checksum = "c469d952047f47f91b68d1cba3f10d63c11d73e4636f24f08daf0278abf01c4d" dependencies = [ "num-traits", ] @@ -323,9 +314,9 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "darling" -version = "0.20.9" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" dependencies = [ "darling_core", "darling_macro", @@ -333,9 +324,9 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.9" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", @@ -347,9 +338,9 @@ dependencies = [ [[package]] name = "darling_macro" -version = "0.20.9" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", "quote", @@ -358,9 +349,18 @@ dependencies = [ [[package]] name = "defmt" -version = "0.3.10" +version = "0.3.100" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -368,9 +368,9 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.4.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", @@ -381,40 +381,40 @@ dependencies = [ [[package]] name = "defmt-parser" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", ] [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "24076cc7203c365e7febfcec15d6667a9ef780bd2c5fd3b2a197400df78f299b" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", - "defmt", + "defmt 1.0.1", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", @@ -423,27 +423,27 @@ dependencies = [ [[package]] name = "document-features" -version = "0.2.8" +version = "0.2.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" dependencies = [ "litrs", ] [[package]] name = "either" -version = "1.12.0" +version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3dca9240753cf90908d7e4aac30f630662b02aebaa1b58a3cadabdb23385b58b" +checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "41fea5ef5bed4d3468dfd44f5c9fa4cda8f54c86d4fb4ae683eacf9d39e2ea12" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", "embassy-futures", + "embassy-hal-internal", "embassy-sync", "embassy-time", "embedded-hal 0.2.7", @@ -456,22 +456,20 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6" +version = "0.8.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-executor-macros", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" +version = "0.7.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "darling", "proc-macro2", @@ -482,45 +480,42 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-hal-internal" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ef3bac31ec146321248a169e9c7b5799f1e0b3829c7a9b324cb4600a7438f59" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "num-traits", ] [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "524eb3c489760508f71360112bca70f6e53173e6fe48fc5f0efd0f5ab217751d" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e1e0bb733acdddbc7097765a47ce80bde2385647cf1d8427331931e06cff9a87" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "aligned", "bit_field", - "bitflags 2.5.0", + "bitflags 2.9.3", "block-device-driver", "cfg-if", "chrono", "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-embedded-hal", "embassy-futures", @@ -545,7 +540,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.3", "sdio-host", "static_assertions", "stm32-fmc", @@ -556,50 +552,46 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" +version = "0.7.1" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", "heapless 0.8.0", ] [[package]] name = "embassy-time" version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f820157f198ada183ad62e0a66f554c610cdcd1a9f27d4b316358103ced7a1f8" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-time-driver", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", - "futures-util", + "futures-core", ] [[package]] name = "embassy-time-driver" version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "embassy-executor", "heapless 0.8.0", @@ -607,21 +599,20 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fc247028eae04174b6635104a35b1ed336aabef4654f5e87a8f32327d231970" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08e753b23799329780c7ac434264026d0422044d6649ed70a73441b14a6436d7" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "embassy-sync", "embassy-usb-driver", ] @@ -676,7 +667,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ - "defmt", + "defmt 0.3.100", ] [[package]] @@ -685,7 +676,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" dependencies = [ - "defmt", + "defmt 0.3.100", "embedded-io", ] @@ -719,12 +710,12 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.9" +version = "0.3.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" +checksum = "778e2ac28f6c47af28e4907f13ffd1e1ddbd400980a9abd7c8df189bf578a5ad" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -765,18 +756,9 @@ dependencies = [ [[package]] name = "glob" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" - -[[package]] -name = "hash32" -version = "0.2.1" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" -dependencies = [ - "byteorder", -] +checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280" [[package]] name = "hash32" @@ -795,24 +777,21 @@ checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" [[package]] name = "heapless" -version = "0.7.17" +version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" dependencies = [ - "atomic-polyfill", - "hash32 0.2.1", - "rustc_version 0.4.0", - "spin", + "hash32", "stable_deref_trait", ] [[package]] name = "heapless" -version = "0.8.0" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5" dependencies = [ - "hash32 0.3.1", + "hash32", "stable_deref_trait", ] @@ -827,18 +806,18 @@ dependencies = [ [[package]] name = "home" -version = "0.5.9" +version = "0.5.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +checksum = "589533453244b0995c858700322199b2becb13b627df2851f64a2775d024abcf" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] name = "humantime" -version = "2.1.0" +version = "2.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" +checksum = "9b112acc8b3adf4b107a8ec20977da0273a8c386765a3ec0229bd500a1443f9f" [[package]] name = "ident_case" @@ -858,9 +837,9 @@ dependencies = [ [[package]] name = "lazy_static" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lazycell" @@ -870,59 +849,49 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.155" +version = "0.2.175" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" +checksum = "6a82ae493e598baaea5209805c49bbf2ea7de956d50d7da0da1164f9c6d28543" [[package]] name = "libloading" -version = "0.8.3" +version = "0.8.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" +checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667" dependencies = [ "cfg-if", - "windows-targets", + "windows-targets 0.53.3", ] [[package]] name = "libm" -version = "0.2.11" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" [[package]] name = "linux-raw-sys" -version = "0.4.14" +version = "0.4.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" +checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" [[package]] name = "litrs" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" - -[[package]] -name = "lock_api" -version = "0.4.12" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" -dependencies = [ - "autocfg", - "scopeguard", -] +checksum = "f5e54036fe321fd421e10d732f155734c4e4afd610dd556d9a82833ab3ee0bed" [[package]] name = "log" -version = "0.4.21" +version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" +checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "memchr" -version = "2.7.2" +version = "2.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" +checksum = "32a282da65faaf38286cf3be983213fcf1d2e2a58700e808f83f4ea9a4804bc0" [[package]] name = "minimal-lexical" @@ -932,9 +901,9 @@ checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" [[package]] name = "nalgebra" -version = "0.33.2" +version = "0.34.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b" +checksum = "9cd59afb6639828b33677758314a4a1a745c15c02bc597095b851c8fd915cf49" dependencies = [ "approx", "nalgebra-macros", @@ -947,9 +916,9 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.2" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" dependencies = [ "proc-macro2", "quote", @@ -1021,9 +990,9 @@ dependencies = [ [[package]] name = "once_cell" -version = "1.19.0" +version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" +checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" [[package]] name = "os_str_bytes" @@ -1033,12 +1002,12 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +checksum = "fd402d00b0fb94c5aee000029204a46884b1262e0c443f166d86d2c0747e1a1a" dependencies = [ "cortex-m", - "defmt", + "defmt 1.0.1", ] [[package]] @@ -1055,9 +1024,9 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" [[package]] name = "pin-project-lite" -version = "0.2.14" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" +checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b" [[package]] name = "pin-utils" @@ -1067,9 +1036,9 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" [[package]] name = "portable-atomic" -version = "1.6.0" +version = "1.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" +checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483" [[package]] name = "proc-macro-error-attr2" @@ -1095,18 +1064,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.92" +version = "1.0.101" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "37d3544b3f2748c54e147655edb5025752e2303145b5aefb3c3ea2c78b973bb0" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.36" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -1117,11 +1086,17 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38" + [[package]] name = "regex" -version = "1.10.5" +version = "1.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b91213439dad192326a0d7c6ee3955910425f441d7038e0d6933b0aec5c4517f" +checksum = "23d7fd106d8c02486a8d64e778353d1cffe08ce79ac2e82f540c86d0facf6912" dependencies = [ "aho-corasick", "memchr", @@ -1131,9 +1106,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.7" +version = "0.4.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38caf58cc5ef2fed281f89292ef23f6365465ed9a41b7a7754eb4e26496c92df" +checksum = "6b9458fa0bfeeac22b5ca447c63aaf45f28439a709ccd244698632f9aa6394d6" dependencies = [ "aho-corasick", "memchr", @@ -1142,15 +1117,15 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.4" +version = "0.8.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" +checksum = "caf4aa5b0f434c91fe5c7f1ecb6a5ece2130b02ad2a590589dda5146df959001" [[package]] name = "rgb" -version = "0.8.37" +version = "0.8.52" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "05aaa8004b64fd573fc9d002f4e632d51ad4f026c2b5ba95fcb6c2f32c2c47d8" +checksum = "0c6a884d2998352bb4daf0183589aec883f16a6da1f4dde84d8e2e9a5409a1ce" dependencies = [ "bytemuck", ] @@ -1167,42 +1142,27 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" dependencies = [ - "semver 0.9.0", -] - -[[package]] -name = "rustc_version" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" -dependencies = [ - "semver 1.0.23", + "semver", ] [[package]] name = "rustix" -version = "0.38.34" +version = "0.38.44" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" +checksum = "fdb5bc1ae2baa591800df16c9ca78619bf65c0488b41b96ccec5d11220d8c154" dependencies = [ - "bitflags 2.5.0", + "bitflags 2.9.3", "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.59.0", ] -[[package]] -name = "scopeguard" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" - [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -1213,12 +1173,6 @@ dependencies = [ "semver-parser", ] -[[package]] -name = "semver" -version = "1.0.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" - [[package]] name = "semver-parser" version = "0.7.0" @@ -1254,22 +1208,13 @@ dependencies = [ [[package]] name = "smart-leds-trait" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +checksum = "edeb89c73244414bb0568611690dd095b2358b3fda5bae65ad784806cca00157" dependencies = [ "rgb", ] -[[package]] -name = "spin" -version = "0.9.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" -dependencies = [ - "lock_api", -] - [[package]] name = "stable_deref_trait" version = "1.2.0" @@ -1284,30 +1229,30 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" [[package]] name = "static_cell" -version = "2.1.0" +version = "2.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" +checksum = "0530892bb4fa575ee0da4b86f86c667132a94b74bb72160f58ee5a4afec74c23" dependencies = [ "portable-atomic", ] [[package]] name = "stm32-fmc" -version = "0.3.0" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" +checksum = "c7f0639399e2307c2446c54d91d4f1596343a1e1d5cab605b9cce11d0ab3858c" dependencies = [ "embedded-hal 0.2.7", ] [[package]] name = "stm32-metapac" -version = "16.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +version = "17.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] @@ -1324,9 +1269,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" -version = "2.0.94" +version = "2.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "987bc0be1cdea8b10216bd06e2ca407d40b9543468fafd3ddfb02f36e77f71f3" +checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6" dependencies = [ "proc-macro2", "quote", @@ -1344,24 +1289,24 @@ dependencies = [ [[package]] name = "textwrap" -version = "0.16.1" +version = "0.16.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" +checksum = "c13547615a44dc9c452a8a534638acdf07120d4b6847c8178705da06306a3057" [[package]] name = "thiserror" -version = "2.0.9" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f072643fd0190df67a8bab670c20ef5d8737177d6ac6b2e9a236cb096206b2cc" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.9" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b50fa271071aae2e6ee85f842e2e28ba8cd2c5fb67f11fcb1fd70b276f9e7d4" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -1376,15 +1321,15 @@ checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f" [[package]] name = "unicode-ident" -version = "1.0.12" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unicode-xid" -version = "0.2.4" +version = "0.2.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" +checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" [[package]] name = "vcell" @@ -1437,11 +1382,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.8" +version = "0.1.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" +checksum = "0978bf7171b3d90bac376700cb56d606feb40f251a475a5d6634613564460b22" dependencies = [ - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -1450,95 +1395,155 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" +[[package]] +name = "windows-link" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a" + [[package]] name = "windows-sys" -version = "0.52.0" +version = "0.59.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" dependencies = [ - "windows-targets", + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2f500e4d28234f72040990ec9d39e3a6b950f9f22d3dba18416c35882612bcb" +dependencies = [ + "windows-targets 0.53.3", ] [[package]] name = "windows-targets" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm 0.52.6", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", ] +[[package]] +name = "windows-targets" +version = "0.53.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5fe6031c4041849d7c496a8ded650796e7b6ecc19df1a431c1a363342e5dc91" +dependencies = [ + "windows-link", + "windows_aarch64_gnullvm 0.53.0", + "windows_aarch64_msvc 0.53.0", + "windows_i686_gnu 0.53.0", + "windows_i686_gnullvm 0.53.0", + "windows_i686_msvc 0.53.0", + "windows_x86_64_gnu 0.53.0", + "windows_x86_64_gnullvm 0.53.0", + "windows_x86_64_msvc 0.53.0", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + [[package]] name = "windows_aarch64_gnullvm" -version = "0.52.5" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" [[package]] name = "windows_aarch64_msvc" -version = "0.52.5" +version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" +checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c" [[package]] name = "windows_i686_gnu" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnu" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3" [[package]] name = "windows_i686_gnullvm" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11" [[package]] name = "windows_i686_msvc" -version = "0.52.5" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" [[package]] -name = "windows_x86_64_gnu" -version = "0.52.5" +name = "windows_i686_msvc" +version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" +checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d" [[package]] -name = "windows_x86_64_gnullvm" -version = "0.52.5" +name = "windows_x86_64_gnu" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" [[package]] -name = "windows_x86_64_msvc" -version = "0.52.5" +name = "windows_x86_64_gnu" +version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" +checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba" -[[patch.unused]] -name = "embassy-executor" -version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" -[[patch.unused]] -name = "embassy-stm32" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57" -[[patch.unused]] -name = "embassy-sync" -version = "0.6.0" -source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" -[[patch.unused]] -name = "embassy-time" -version = "0.3.1" -source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" +[[package]] +name = "windows_x86_64_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index c7f2c65e..9e87cf88 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -8,40 +8,40 @@ edition = "2021" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.7.0", features = [ +embassy-executor = { version = "0.8.0", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", - "task-arena-size-32768", ] } embassy-time = { version = "0.4.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.2.0", features = [ +embassy-stm32 = { version = "0.3.0", features = [ "defmt", "stm32g474ve", + "dual-bank", # CHECK, this is probably critical "unstable-pac", "time-driver-tim1", "exti", "chrono" ] } embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.6.2" } +embassy-sync = { version = "0.7.1" } -defmt = "=0.3.10" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) -defmt-rtt = "0.4.1" -panic-probe = { version = "0.3", features = ["print-defmt"] } +defmt = "1.0.1" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "1.0.0" +panic-probe = { version = "1.0.0", features = ["print-defmt"] } futures-util = { version = "0.3.31", default-features = false } -static_cell = "2.0.0" +static_cell = "2.1.1" critical-section = "1.2.0" const_format = "0.2.34" -heapless = "0.7.16" -libm = "0.2.11" -nalgebra = { version = "0.33.2", default-features = false, features = [ +heapless = "0.9.1" +libm = "0.2.15" +nalgebra = { version = "0.34.0", default-features = false, features = [ "libm", "macros", ] } @@ -51,7 +51,7 @@ ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } [dev-dependencies] -defmt-test = "0.3.0" +defmt-test = "0.4.0" [profile.dev] # opt-level = 3 diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 9d269294..1bdca500 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -11,7 +11,7 @@ use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, spi::{Config, Spi}, - time::mhz, + time::mhz, Peri, }; use embassy_time::{Duration, Timer}; @@ -25,16 +25,16 @@ use panic_probe as _; #[embassy_executor::task] async fn blink( - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - status_led_blue1: BlueStatusLedPin, - usr_btn_pin: UserBtnPin, + reg_charge: Peri<'static, ChargePin>, + status_led_red: Peri<'static, RedStatusLedPin>, + status_led_green: Peri<'static, GreenStatusLedPin>, + status_led_blue1: Peri<'static, BlueStatusLedPin>, + usr_btn_pin: Peri<'static, UserBtnPin>, mut adc: Adc<'static, PowerRailAdc>, - mut rail_200v_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - mut rail_5v0_pin: PowerRail5v0ReadPin, - mut rail_3v3_pin: PowerRail3v3ReadPin, + mut rail_200v_pin: Peri<'static, PowerRail200vReadPin>, + mut rail_12v0_pin: Peri<'static, PowerRailVswReadPin>, + mut rail_5v0_pin: Peri<'static, PowerRail5v0ReadPin>, + mut rail_3v3_pin: Peri<'static, PowerRail3v3ReadPin>, ) -> ! { let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::High, Speed::Medium); @@ -94,10 +94,10 @@ async fn blink( #[embassy_executor::task] async fn dotstar_lerp_task( - dotstar_spi: DotstarSpi, - dotstar_mosi_pin: DotstarSpiMosiPin, - dotstar_sck_pin: DotstarSpiSckPin, - dotstar_tx_dma: DotstarSpiTxDma, + dotstar_spi: Peri<'static, DotstarSpi>, + dotstar_mosi_pin: Peri<'static, DotstarSpiMosiPin>, + dotstar_sck_pin: Peri<'static, DotstarSpiSckPin>, + dotstar_tx_dma: Peri<'static, DotstarSpiTxDma>, ) { let mut dotstar_spi_config = Config::default(); dotstar_spi_config.frequency = mhz(1); diff --git a/kicker-board/src/bin/hwtest-breakbeam/main.rs b/kicker-board/src/bin/hwtest-breakbeam/main.rs index 386c4fcc..14393d16 100644 --- a/kicker-board/src/bin/hwtest-breakbeam/main.rs +++ b/kicker-board/src/bin/hwtest-breakbeam/main.rs @@ -29,7 +29,7 @@ async fn main(_spawner: Spawner) { // Breakbeam // nets on schematic are inverted to silkscreen, sorry :/ -Will - let mut breakbeam = Breakbeam::new(p.PA1, p.PA0); + let mut breakbeam = Breakbeam::new(p.PA1.into(), p.PA0.into()); status_led_green.set_high(); Timer::after(Duration::from_millis(250)).await; diff --git a/kicker-board/src/bin/hwtest-charge/main.rs b/kicker-board/src/bin/hwtest-charge/main.rs index ebeb6b6d..3ea870d6 100644 --- a/kicker-board/src/bin/hwtest-charge/main.rs +++ b/kicker-board/src/bin/hwtest-charge/main.rs @@ -10,9 +10,8 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ - adc::Adc, - adc::SampleTime, - gpio::{Level, Output, Speed}, + adc::{Adc, SampleTime}, + gpio::{Level, Output, Speed}, Peri, }; use embassy_time::{Duration, Ticker, Timer}; @@ -24,12 +23,12 @@ use ateam_kicker_board::*; #[embassy_executor::task] async fn run_kick( mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_vsw_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - kick_pin: KickPin, + mut hv_pin: Peri<'static, PowerRail200vReadPin>, + mut rail_vsw_pin: Peri<'static, PowerRailVswReadPin>, + reg_charge: Peri<'static, ChargePin>, + status_led_red: Peri<'static, RedStatusLedPin>, + status_led_green: Peri<'static, GreenStatusLedPin>, + kick_pin: Peri<'static, KickPin>, ) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index d1a0c525..3f5c6b58 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -12,10 +12,9 @@ use embassy_executor::{Executor, InterruptExecutor, Spawner}; use embassy_stm32::{ adc::{Adc, Resolution, SampleTime}, gpio::{Level, Output, Speed}, - interrupt, - interrupt::InterruptExt, + interrupt::{self, InterruptExt}, pac::Interrupt, - usart::{Config, Parity, StopBits, Uart}, + usart::{Config, Parity, StopBits, Uart}, Peri, }; use embassy_stm32::{bind_interrupts, peripherals, usart}; @@ -55,12 +54,12 @@ async fn high_pri_kick_task( coms_reader: &'static UartReadQueue, coms_writer: &'static UartWriteQueue, mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - mut rail_pin: PowerRail200vReadPin, - err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin, + charge_pin: Peri<'static, ChargePin>, + kick_pin: Peri<'static, KickPin>, + chip_pin: Peri<'static, ChipPin>, + mut rail_pin: Peri<'static, PowerRail200vReadPin>, + err_led_pin: Peri<'static, RedStatusLedPin>, + ball_detected_led_pin: Peri<'static, BlueStatusLedPin>, ) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -194,7 +193,7 @@ async fn high_pri_kick_task( static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static _EXECUTOR_LOW: StaticCell = StaticCell::new(); -#[interrupt] +#[embassy_stm32::interrupt] unsafe fn TIM2() { EXECUTOR_HIGH.on_interrupt(); } diff --git a/kicker-board/src/bin/hwtest-dribbler/main.rs b/kicker-board/src/bin/hwtest-dribbler/main.rs index 88b068e9..4e64cd13 100644 --- a/kicker-board/src/bin/hwtest-dribbler/main.rs +++ b/kicker-board/src/bin/hwtest-dribbler/main.rs @@ -77,8 +77,8 @@ async fn main(_spawner: Spawner) -> ! { &DRIB_IDLE_BUFFERED_UART, DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PE13, - p.PE14, + p.PE13.into(), + p.PE14.into(), Pull::None, true, ); diff --git a/kicker-board/src/bin/hwtest-flash/main.rs b/kicker-board/src/bin/hwtest-flash/main.rs index cf27f729..498d5367 100644 --- a/kicker-board/src/bin/hwtest-flash/main.rs +++ b/kicker-board/src/bin/hwtest-flash/main.rs @@ -41,7 +41,7 @@ async fn main(_spawner: Spawner) -> ! { let rx_buf = FLASH_RX_BUF.init([0; 256]); let tx_buf = FLASH_TX_BUF.init([0; 256]); - let mut flash: AT25DF041B<'static, true> = AT25DF041B::new(spi, p.PB12, rx_buf, tx_buf); + let mut flash: AT25DF041B<'static, true> = AT25DF041B::new(spi, p.PB12.into(), rx_buf, tx_buf); let res = flash.verify_chip_id().await; if res.is_err() { defmt::error!("failed to verify flash chip ID"); diff --git a/kicker-board/src/bin/hwtest-kick/main.rs b/kicker-board/src/bin/hwtest-kick/main.rs index a0ddec78..28cabb96 100644 --- a/kicker-board/src/bin/hwtest-kick/main.rs +++ b/kicker-board/src/bin/hwtest-kick/main.rs @@ -12,7 +12,7 @@ use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, - opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, Peri, }; use embassy_time::{Duration, Ticker, Timer}; @@ -24,14 +24,14 @@ use ateam_kicker_board::*; #[embassy_executor::task] async fn run_kick( mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - usr_btn_pin: UserBtnPin, + mut hv_pin: Peri<'static, PowerRail200vReadPin>, + mut rail_12v0_pin: Peri<'static, PowerRailVswReadPin>, + reg_charge: Peri<'static, ChargePin>, + status_led_red: Peri<'static, RedStatusLedPin>, + status_led_green: Peri<'static, GreenStatusLedPin>, + usr_btn_pin: Peri<'static, UserBtnPin>, // chip_pin: ChipPin, - kick_pin: KickPin, + kick_pin: Peri<'static, KickPin>, ) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); @@ -145,7 +145,7 @@ fn main() -> ! { let _vsw_en = Output::new(p.PE10, Level::High, Speed::Medium); let mut hv_opamp_inst = OpAmp::new(p.OPAMP3, OpAmpSpeed::HighSpeed); - let _hv_opamp = hv_opamp_inst.buffer_ext(p.PB0, p.PB1, OpAmpGain::Mul2); + let _hv_opamp = hv_opamp_inst.pga_ext(p.PB0, p.PB1, OpAmpGain::Mul2); let mut adc = Adc::new(p.ADC1); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); diff --git a/kicker-board/src/bin/hwtest-kickstr/main.rs b/kicker-board/src/bin/hwtest-kickstr/main.rs index 591fb131..b7826133 100644 --- a/kicker-board/src/bin/hwtest-kickstr/main.rs +++ b/kicker-board/src/bin/hwtest-kickstr/main.rs @@ -20,7 +20,7 @@ use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, - opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, Peri, }; use embassy_time::{Duration, Ticker, Timer}; @@ -32,13 +32,13 @@ use ateam_kicker_board::*; #[embassy_executor::task] async fn run_kick( mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRailVswReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - usr_btn_pin: UserBtnPin, - kick_pin: KickPin, + mut hv_pin: Peri<'static, PowerRail200vReadPin>, + mut rail_12v0_pin: Peri<'static, PowerRailVswReadPin>, + reg_charge: Peri<'static, ChargePin>, + status_led_red: Peri<'static, RedStatusLedPin>, + status_led_green: Peri<'static, GreenStatusLedPin>, + usr_btn_pin: Peri<'static, UserBtnPin>, + kick_pin: Peri<'static, KickPin>, ) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); @@ -159,7 +159,7 @@ fn main() -> ! { let _vsw_en = Output::new(p.PE10, Level::High, Speed::Medium); let mut hv_opamp_inst = OpAmp::new(p.OPAMP3, OpAmpSpeed::HighSpeed); - let _hv_opamp = hv_opamp_inst.buffer_ext(p.PB0, p.PB1, OpAmpGain::Mul2); + let _hv_opamp = hv_opamp_inst.pga_ext(p.PB0, p.PB1, OpAmpGain::Mul2); let mut adc = Adc::new(p.ADC1); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index d8a0103e..fac2771f 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -15,6 +15,7 @@ use ateam_kicker_board::{ }; use defmt::*; +use embassy_stm32::Peri; use embassy_sync::{ blocking_mutex::raw::CriticalSectionRawMutex, pubsub::{PubSubChannel, Publisher, Subscriber}, @@ -104,15 +105,15 @@ async fn high_pri_kick_task( coms_reader: &'static UartReadQueue, coms_writer: &'static UartWriteQueue, mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - breakbeam_tx: BreakbeamRightAgpioPin, - breakbeam_rx: BreakbeamLeftAgpioPin, - mut rail_pin: PowerRail200vReadPin, - grn_led_pin: GreenStatusLedPin, - err_led_pin: RedStatusLedPin, - ball_detected_led1_pin: BlueStatusLedPin, + charge_pin: Peri<'static, ChargePin>, + kick_pin: Peri<'static, KickPin>, + chip_pin: Peri<'static, ChipPin>, + breakbeam_tx: Peri<'static, BreakbeamRightAgpioPin>, + breakbeam_rx: Peri<'static, BreakbeamLeftAgpioPin>, + mut rail_pin: Peri<'static, PowerRail200vReadPin>, + grn_led_pin: Peri<'static, GreenStatusLedPin>, + err_led_pin: Peri<'static, RedStatusLedPin>, + ball_detected_led1_pin: Peri<'static, BlueStatusLedPin>, drib_vel_pub: Publisher<'static, CriticalSectionRawMutex, f32, 1, 1, 1>, mut drib_telem_sub: Subscriber<'static, CriticalSectionRawMutex, MotorTelemetry, 1, 1, 1>, ) -> ! { @@ -132,7 +133,7 @@ async fn high_pri_kick_task( // TODO dotstars - let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); + let mut breakbeam = Breakbeam::new(breakbeam_tx.into(), breakbeam_rx.into()); // coms buffers let mut telemetry_enabled: bool; // = false; @@ -555,7 +556,7 @@ async fn main(spawner: Spawner) -> ! { // enable opamp for hv measurement let mut hv_opamp_inst = OpAmp::new(p.OPAMP3, OpAmpSpeed::HighSpeed); - let _hv_opamp = hv_opamp_inst.buffer_ext(p.PB0, p.PB1, OpAmpGain::Mul2); + let _hv_opamp = hv_opamp_inst.pga_ext(p.PB0, p.PB1, OpAmpGain::Mul2); // config ADC let mut adc = Adc::new(p.ADC1); @@ -643,8 +644,8 @@ async fn main(spawner: Spawner) -> ! { &DRIB_IDLE_BUFFERED_UART, DRIB_IDLE_BUFFERED_UART.get_uart_read_queue(), DRIB_IDLE_BUFFERED_UART.get_uart_write_queue(), - p.PE13, - p.PE14, + p.PE13.into(), + p.PE14.into(), Pull::None, true, ); diff --git a/kicker-board/src/drivers/breakbeam.rs b/kicker-board/src/drivers/breakbeam.rs index 43a03f6c..1473c678 100644 --- a/kicker-board/src/drivers/breakbeam.rs +++ b/kicker-board/src/drivers/breakbeam.rs @@ -1,6 +1,5 @@ use embassy_stm32::{ - gpio::{Input, Level, Output, Pin, Pull, Speed}, - Peripheral, + gpio::{AnyPin, Input, Level, Output, Pull, Speed}, Peri, }; pub struct Breakbeam<'a> { @@ -10,8 +9,8 @@ pub struct Breakbeam<'a> { impl<'a> Breakbeam<'a> { pub fn new( - pin_tx: impl Peripheral

+ 'a, - pin_rx: impl Peripheral

+ 'a, + pin_tx: Peri<'a, AnyPin>, + pin_rx: Peri<'a, AnyPin>, ) -> Self { let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); let pin_rx = Input::new(pin_rx, Pull::Down); diff --git a/kicker-board/src/drivers/mod.rs b/kicker-board/src/drivers/mod.rs index cb7b7477..7875acd6 100644 --- a/kicker-board/src/drivers/mod.rs +++ b/kicker-board/src/drivers/mod.rs @@ -13,8 +13,8 @@ use ateam_lib_stm32::{ uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, }; use embassy_stm32::{ - gpio::{Pin, Pull}, - usart::Parity, + gpio::{AnyPin, Pull}, + usart::Parity, Peri, }; use embassy_time::{with_timeout, Duration, Timer}; @@ -91,8 +91,8 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: impl Pin, - reset_pin: impl Pin, + boot0_pin: Peri<'a, AnyPin>, + reset_pin: Peri<'a, AnyPin>, firmware_image: &'a [u8], ball_detected_thresh: f32, ) -> DribblerMotor<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { diff --git a/kicker-board/src/lib.rs b/kicker-board/src/lib.rs index 682ee574..92a1cd5d 100644 --- a/kicker-board/src/lib.rs +++ b/kicker-board/src/lib.rs @@ -1,7 +1,6 @@ #![no_std] #![feature(type_alias_impl_trait)] #![feature(maybe_uninit_slice)] -#![feature(maybe_uninit_uninit_array)] #![feature(sync_unsafe_cell)] pub mod drivers; diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index d8b4e4d4..a65f971a 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -25,14 +25,14 @@ name = "ateam-lib-stm32" version = "0.1.0" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "embassy-executor", "embassy-futures", "embassy-stm32", "embassy-sync", "embassy-time", - "heapless 0.8.0", + "heapless 0.9.1", "num-traits", "paste", "smart-leds", @@ -47,7 +47,7 @@ dependencies = [ "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "defmt-test", "embassy-executor", @@ -56,26 +56,17 @@ dependencies = [ "embassy-sync", "embassy-time", "futures-util", - "heapless 0.7.17", + "heapless 0.9.1", "libm", "panic-probe", "static_cell", ] -[[package]] -name = "atomic-polyfill" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" -dependencies = [ - "critical-section", -] - [[package]] name = "autocfg" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bare-metal" @@ -83,7 +74,7 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" dependencies = [ - "rustc_version 0.2.3", + "rustc_version", ] [[package]] @@ -106,9 +97,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.8.0" +version = "2.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36" +checksum = "34efbcccd345379ca2868b2b2c9d3782e9cc58ba87bc7d79d5b53d9c9ae6f25d" [[package]] name = "block-device-driver" @@ -121,9 +112,9 @@ dependencies = [ [[package]] name = "bytemuck" -version = "1.21.0" +version = "1.23.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" +checksum = "3995eaeebcdf32f91f980d360f78732ddc061097ab4e39991ae7a6ace9194677" [[package]] name = "byteorder" @@ -133,15 +124,15 @@ checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9" [[package]] name = "chrono" -version = "0.4.39" +version = "0.4.41" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825" +checksum = "c469d952047f47f91b68d1cba3f10d63c11d73e4636f24f08daf0278abf01c4d" dependencies = [ "num-traits", ] @@ -216,9 +207,9 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "darling" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" dependencies = [ "darling_core", "darling_macro", @@ -226,9 +217,9 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", @@ -240,9 +231,9 @@ dependencies = [ [[package]] name = "darling_macro" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", "quote", @@ -251,9 +242,18 @@ dependencies = [ [[package]] name = "defmt" -version = "0.3.10" +version = "0.3.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -261,9 +261,9 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.4.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", @@ -274,40 +274,40 @@ dependencies = [ [[package]] name = "defmt-parser" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", ] [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "24076cc7203c365e7febfcec15d6667a9ef780bd2c5fd3b2a197400df78f299b" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", - "defmt", + "defmt 1.0.1", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", @@ -316,20 +316,21 @@ dependencies = [ [[package]] name = "document-features" -version = "0.2.10" +version = "0.2.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb6969eaabd2421f8a2775cfd2471a2b634372b4a25d41e3bd647b79912850a0" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" dependencies = [ "litrs", ] [[package]] name = "embassy-embedded-hal" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", "embassy-futures", + "embassy-hal-internal", "embassy-sync", "embassy-time", "embedded-hal 0.2.7", @@ -342,20 +343,20 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.8.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-executor-macros", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.7.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "darling", "proc-macro2", @@ -366,42 +367,42 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-hal-internal" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "num-traits", ] [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "aligned", "bit_field", - "bitflags 2.8.0", + "bitflags 2.9.3", "block-device-driver", "cfg-if", "chrono", "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-embedded-hal", "embassy-futures", @@ -426,7 +427,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.3", "sdio-host", "static_assertions", "stm32-fmc", @@ -437,46 +439,46 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.7.1" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", "heapless 0.8.0", ] [[package]] name = "embassy-time" version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-time-driver", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", - "futures-util", + "futures-core", ] [[package]] name = "embassy-time-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "embassy-executor", "heapless 0.8.0", @@ -484,19 +486,20 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#195b1a593a4f51ebc7ae61b5045c620a1dbd25d8" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "embassy-sync", "embassy-usb-driver", ] @@ -551,7 +554,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ - "defmt", + "defmt 0.3.100", ] [[package]] @@ -560,7 +563,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" dependencies = [ - "defmt", + "defmt 0.3.100", "embedded-io", ] @@ -615,15 +618,6 @@ dependencies = [ "pin-utils", ] -[[package]] -name = "hash32" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" -dependencies = [ - "byteorder", -] - [[package]] name = "hash32" version = "0.3.1" @@ -635,24 +629,21 @@ dependencies = [ [[package]] name = "heapless" -version = "0.7.17" +version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" dependencies = [ - "atomic-polyfill", - "hash32 0.2.1", - "rustc_version 0.4.1", - "spin", + "hash32", "stable_deref_trait", ] [[package]] name = "heapless" -version = "0.8.0" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5" dependencies = [ - "hash32 0.3.1", + "hash32", "stable_deref_trait", ] @@ -664,25 +655,15 @@ checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" [[package]] name = "libm" -version = "0.2.11" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" [[package]] name = "litrs" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" - -[[package]] -name = "lock_api" -version = "0.4.12" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" -dependencies = [ - "autocfg", - "scopeguard", -] +checksum = "f5e54036fe321fd421e10d732f155734c4e4afd610dd556d9a82833ab3ee0bed" [[package]] name = "nb" @@ -710,12 +691,12 @@ dependencies = [ [[package]] name = "panic-probe" -version = "0.3.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +checksum = "fd402d00b0fb94c5aee000029204a46884b1262e0c443f166d86d2c0747e1a1a" dependencies = [ "cortex-m", - "defmt", + "defmt 1.0.1", ] [[package]] @@ -738,9 +719,9 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" [[package]] name = "portable-atomic" -version = "1.10.0" +version = "1.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6" +checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483" [[package]] name = "proc-macro-error-attr2" @@ -766,18 +747,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.93" +version = "1.0.101" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.38" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -788,11 +769,17 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38" + [[package]] name = "rgb" -version = "0.8.50" +version = "0.8.52" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57397d16646700483b67d2dd6511d79318f9d057fdbd21a4066aeac8b41d310a" +checksum = "0c6a884d2998352bb4daf0183589aec883f16a6da1f4dde84d8e2e9a5409a1ce" dependencies = [ "bytemuck", ] @@ -803,29 +790,14 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" dependencies = [ - "semver 0.9.0", + "semver", ] -[[package]] -name = "rustc_version" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" -dependencies = [ - "semver 1.0.25", -] - -[[package]] -name = "scopeguard" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" - [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -836,12 +808,6 @@ dependencies = [ "semver-parser", ] -[[package]] -name = "semver" -version = "1.0.25" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f79dfe2d285b0488816f30e700a7438c5a73d816b5b7d3ac72fbc48b0d185e03" - [[package]] name = "semver-parser" version = "0.7.0" @@ -859,22 +825,13 @@ dependencies = [ [[package]] name = "smart-leds-trait" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +checksum = "edeb89c73244414bb0568611690dd095b2358b3fda5bae65ad784806cca00157" dependencies = [ "rgb", ] -[[package]] -name = "spin" -version = "0.9.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" -dependencies = [ - "lock_api", -] - [[package]] name = "stable_deref_trait" version = "1.2.0" @@ -889,9 +846,9 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" [[package]] name = "static_cell" -version = "2.1.0" +version = "2.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" +checksum = "0530892bb4fa575ee0da4b86f86c667132a94b74bb72160f58ee5a4afec74c23" dependencies = [ "portable-atomic", ] @@ -907,12 +864,12 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +version = "17.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] @@ -923,9 +880,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" -version = "2.0.96" +version = "2.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d5d0adab1ae378d7f53bdebc67a39f1f151407ef230f0ce2883572f5d8985c80" +checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6" dependencies = [ "proc-macro2", "quote", @@ -934,18 +891,18 @@ dependencies = [ [[package]] name = "thiserror" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d452f284b73e6d76dd36758a0c8684b1d5be31f92b89d07fd5822175732206fc" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26afc1baea8a989337eeb52b6e72a039780ce45c3edfcc9c5b9d112feeb173c2" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -954,9 +911,9 @@ dependencies = [ [[package]] name = "unicode-ident" -version = "1.0.14" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unicode-xid" diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index 236e992c..4b525767 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -8,21 +8,20 @@ repository = "https://github.com/SSL-A-Team/firmware" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -defmt = "0.3.10" -defmt-rtt = "0.4.1" -embassy-executor = { version = "0.7.0", features = [ +defmt = "1.0.1" +defmt-rtt = "1.0.0" +embassy-executor = { version = "0.8.0", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", - "task-arena-size-32768", ] } embassy-time = { version = "0.4.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.2.0", features = [ +embassy-stm32 = { version = "0.3.0", features = [ "defmt", "stm32h723zg", "unstable-pac", @@ -32,18 +31,18 @@ embassy-stm32 = { version = "0.2.0", features = [ ] } embassy-futures = { version = "0.1.0" } futures-util = { version = "0.3.31", default-features = false } -embassy-sync = { version = "0.6.1" } -panic-probe = { version = "0.3", features = ["print-defmt"] } -static_cell = "2.0.0" +embassy-sync = { version = "0.7.1" } +panic-probe = { version = "1.0.0", features = ["print-defmt"] } +static_cell = "2.1.1" critical-section = "1.2.0" const_format = "0.2.34" -heapless = "0.7.16" -libm = "0.2.11" +heapless = "0.9.1" +libm = "0.2.15" ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } [dev-dependencies] -defmt-test = "0.3.0" +defmt-test = "0.4.0" [profile.dev] opt-level = 3 diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs index c1514f9b..242bfa4d 100644 --- a/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs @@ -1,6 +1,5 @@ #![no_std] #![no_main] -#![feature(generic_arg_infer)] use ateam_lib_stm32::drivers::switches::button::{AdvExtiButton, ADV_BTN_EVENT_DOUBLE_TAP}; diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs index e39942df..c528af02 100644 --- a/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs @@ -1,6 +1,5 @@ #![no_std] #![no_main] -#![feature(generic_arg_infer)] use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index 22327119..b6b586a8 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -11,7 +11,7 @@ use embassy_stm32::{ gpio::{Level, Output, Pull, Speed}, interrupt, peripherals::{self, *}, - usart::{self, *}, + usart::{self, *}, Peri, }; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; @@ -133,11 +133,11 @@ async fn tx_task( #[embassy_executor::task] async fn handle_btn_press( - usr_btn_pin: UserBtnPin, - usr_btn_exti: UserBtnExti, - led_green_pin: LedGreenPin, - led_yellow_pin: LedYellowPin, - led_red_pin: LedRedPin, + usr_btn_pin: Peri<'static, UserBtnPin>, + usr_btn_exti: Peri<'static, UserBtnExti>, + led_green_pin: Peri<'static, LedGreenPin>, + led_yellow_pin: Peri<'static, LedYellowPin>, + led_red_pin: Peri<'static, LedRedPin>, coms_writer: &'static IdleBufferedUart< MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index 7da013b4..a62117be 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -12,7 +12,7 @@ use embassy_stm32::{ interrupt, pac::Interrupt, peripherals::{self, *}, - usart::{self, *}, + usart::{self, *}, Peri, }; use embassy_time::Timer; @@ -113,11 +113,11 @@ async fn tx_task( #[embassy_executor::task] async fn handle_btn_press( - usr_btn_pin: UserBtnPin, - usr_btn_exti: UserBtnExti, - led_green_pin: LedGreenPin, - led_yellow_pin: LedYellowPin, - led_red_pin: LedRedPin, + usr_btn_pin: Peri<'static, UserBtnPin>, + usr_btn_exti: Peri<'static, UserBtnExti>, + led_green_pin: Peri<'static, LedGreenPin>, + led_yellow_pin: Peri<'static, LedYellowPin>, + led_red_pin: Peri<'static, LedRedPin>, ) { let mut usr_btn = ExtiInput::new(usr_btn_pin, usr_btn_exti, Pull::Down); diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 6d276654..3a80eeb6 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -33,7 +33,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", - "heapless", + "heapless 0.9.1", "num-traits", "panic-probe", "paste", @@ -42,9 +42,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bare-metal" @@ -75,9 +75,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.8.0" +version = "2.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36" +checksum = "34efbcccd345379ca2868b2b2c9d3782e9cc58ba87bc7d79d5b53d9c9ae6f25d" [[package]] name = "block-device-driver" @@ -90,9 +90,9 @@ dependencies = [ [[package]] name = "bytemuck" -version = "1.21.0" +version = "1.23.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" +checksum = "3995eaeebcdf32f91f980d360f78732ddc061097ab4e39991ae7a6ace9194677" [[package]] name = "byteorder" @@ -102,9 +102,9 @@ checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9" [[package]] name = "cortex-m" @@ -155,9 +155,9 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "darling" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" dependencies = [ "darling_core", "darling_macro", @@ -165,9 +165,9 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", @@ -179,9 +179,9 @@ dependencies = [ [[package]] name = "darling_macro" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", "quote", @@ -190,9 +190,9 @@ dependencies = [ [[package]] name = "defmt" -version = "0.3.10" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -200,9 +200,9 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.4.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", @@ -213,18 +213,18 @@ dependencies = [ [[package]] name = "defmt-parser" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" dependencies = [ "critical-section", "defmt", @@ -232,9 +232,9 @@ dependencies = [ [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "24076cc7203c365e7febfcec15d6667a9ef780bd2c5fd3b2a197400df78f299b" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", @@ -244,9 +244,9 @@ dependencies = [ [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", @@ -264,8 +264,8 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "embassy-futures", "embassy-hal-internal", @@ -280,8 +280,8 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.8.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", "document-features", @@ -290,8 +290,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.7.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "darling", "proc-macro2", @@ -302,12 +302,12 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-hal-internal" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", @@ -317,16 +317,16 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-stm32" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "aligned", "bit_field", - "bitflags 2.8.0", + "bitflags 2.9.3", "block-device-driver", "cfg-if", "cortex-m", @@ -353,7 +353,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.3", "sdio-host", "static_assertions", "stm32-fmc", @@ -364,21 +365,21 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.7.1" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", - "heapless", + "heapless 0.8.0", ] [[package]] name = "embassy-time" version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", @@ -387,26 +388,29 @@ dependencies = [ "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", - "futures-util", + "futures-core", ] [[package]] name = "embassy-time-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "document-features", ] [[package]] name = "embassy-usb-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +dependencies = [ + "embedded-io-async", +] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#897d42e01214396b448e5aae06a4186a4c282e7a" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", "embassy-sync", @@ -542,6 +546,16 @@ dependencies = [ "stable_deref_trait", ] +[[package]] +name = "heapless" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5" +dependencies = [ + "hash32", + "stable_deref_trait", +] + [[package]] name = "ident_case" version = "1.0.1" @@ -550,9 +564,9 @@ checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" [[package]] name = "litrs" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +checksum = "f5e54036fe321fd421e10d732f155734c4e4afd610dd556d9a82833ab3ee0bed" [[package]] name = "nb" @@ -580,9 +594,9 @@ dependencies = [ [[package]] name = "panic-probe" -version = "0.3.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +checksum = "fd402d00b0fb94c5aee000029204a46884b1262e0c443f166d86d2c0747e1a1a" dependencies = [ "cortex-m", "defmt", @@ -630,18 +644,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.93" +version = "1.0.101" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.38" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -652,11 +666,17 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38" + [[package]] name = "rgb" -version = "0.8.50" +version = "0.8.52" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57397d16646700483b67d2dd6511d79318f9d057fdbd21a4066aeac8b41d310a" +checksum = "0c6a884d2998352bb4daf0183589aec883f16a6da1f4dde84d8e2e9a5409a1ce" dependencies = [ "bytemuck", ] @@ -672,9 +692,9 @@ dependencies = [ [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -732,9 +752,8 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +version = "17.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" dependencies = [ "cortex-m", ] @@ -747,9 +766,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" -version = "2.0.98" +version = "2.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "36147f1a48ae0ec2b5b3bc5b537d267457555a10dc06f3dbc8cb11ba3006d3b1" +checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6" dependencies = [ "proc-macro2", "quote", @@ -758,18 +777,18 @@ dependencies = [ [[package]] name = "thiserror" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d452f284b73e6d76dd36758a0c8684b1d5be31f92b89d07fd5822175732206fc" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26afc1baea8a989337eeb52b6e72a039780ce45c3edfcc9c5b9d112feeb173c2" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -778,9 +797,9 @@ dependencies = [ [[package]] name = "unicode-ident" -version = "1.0.16" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a210d160f08b701c8721ba1c726c11662f877ea6b7094007e1ca9a1041945034" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "vcell" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index cebdb723..0f7e2c55 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -7,18 +7,18 @@ description = "A common library for ateam stm32 targets" repository = "https://github.com/SSL-A-Team/firmware" [dependencies] -embassy-stm32 = { version = "0.2.0", default-features = false, features = ["exti"]} -embassy-executor = { version = "0.7.0", default-features = false } -embassy-sync = { version = "0.6.2" } +embassy-stm32 = { version = "0.3.0", default-features = false, features = ["exti"]} +embassy-executor = { version = "0.8.0", default-features = false } +embassy-sync = { version = "0.7.1" } embassy-futures = { version = "0.1.0" } embassy-time = { version = "0.4.0" } paste = { version = "1.0.15" } -defmt = "0.3.10" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) -defmt-rtt = "0.4.1" +defmt = "1.0.1" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "1.0.0" critical-section = "1.1.1" -heapless = "0.8.0" +heapless = "0.9.1" num-traits = { version = "0.2.19", default-features = false } smart-leds = "0.4.0" @@ -35,8 +35,8 @@ embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} embassy-futures = { git = "https://github.com/embassy-rs/embassy"} [dev-dependencies] -defmt-test = "0.3.0" -panic-probe = { version = "0.3", features = ["print-defmt"] } +defmt-test = "0.4.0" +panic-probe = { version = "1.0.0", features = ["print-defmt"] } [lib] test = false diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index 2cb0c6d2..34500efa 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -2,8 +2,9 @@ use core::cmp::min; use defmt_rtt as _; -use embassy_stm32::gpio::{Level, Output, Pin, Pull, Speed}; +use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_stm32::usart::{self, Config, DataBits, Parity, StopBits}; +use embassy_stm32::Peri; use embassy_time::with_timeout; use embassy_time::{Duration, Timer}; @@ -88,8 +89,8 @@ impl< uart: &'a IdleBufferedUart, read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: impl Pin, - reset_pin: impl Pin, + boot0_pin: Peri<'a, AnyPin>, + reset_pin: Peri<'a, AnyPin>, _reset_pin_pull: Pull, reset_polarity_high: bool, ) -> Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_UART_QUEUES> { diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index 01bcd687..74b19959 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -1,7 +1,7 @@ use embassy_stm32::{ - gpio::{Level, Output, Pin, Speed}, + gpio::{AnyPin, Level, Output, Speed}, mode::Async, - spi::{self, Error}, + spi::{self, Error}, Peri, }; pub struct AT25DF041B<'buf, const CS_POL_N: bool> { @@ -14,7 +14,7 @@ pub struct AT25DF041B<'buf, const CS_POL_N: bool> { impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { pub fn new( spi: spi::Spi<'static, Async>, - chip_select: impl Pin, + chip_select: Peri<'static, AnyPin>, tx_buf: &'buf mut [u8; 256], rx_buf: &'buf mut [u8; 256], ) -> AT25DF041B<'buf, CS_POL_N> { diff --git a/lib-stm32/src/drivers/imu/bmi085.rs b/lib-stm32/src/drivers/imu/bmi085.rs index 0e2c0596..b927da1b 100644 --- a/lib-stm32/src/drivers/imu/bmi085.rs +++ b/lib-stm32/src/drivers/imu/bmi085.rs @@ -8,11 +8,11 @@ use core::{cmp::min, f32::consts::PI}; use embassy_stm32::{ - gpio::{Level, Output, Pin, Speed}, + gpio::{AnyPin, Level, Output, Speed}, mode::Async, spi::{self, MisoPin, MosiPin, SckPin}, time::hz, - Peripheral, + Peri, }; use embassy_time::{Duration, Timer}; @@ -219,14 +219,14 @@ impl<'a, 'buf> Bmi085<'a, 'buf> { ///t creates a new BMI085 instance from uninitialized pins pub fn new_from_pins( - peri: impl Peripheral

+ 'a, - sck: impl Peripheral

> + 'a, - mosi: impl Peripheral

> + 'a, - miso: impl Peripheral

> + 'a, - txdma: impl Peripheral

> + 'a, - rxdma: impl Peripheral

> + 'a, - accel_cs: impl Pin, - gyro_cs: impl Pin, + peri: Peri<'a, SpiPeri>, + sck: Peri<'a, impl SckPin>, + mosi: Peri<'a, impl MosiPin>, + miso: Peri<'a, impl MisoPin>, + txdma: Peri<'a, impl spi::TxDma>, + rxdma: Peri<'a, impl spi::RxDma>, + accel_cs: Peri<'a, AnyPin>, + gyro_cs: Peri<'a, AnyPin>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { let mut spi_config = spi::Config::default(); diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 8f066029..7a9d70f2 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -7,11 +7,10 @@ use core::{cmp::min, f32::consts::PI}; use embassy_stm32::{ - gpio::{Level, Output, Pin, Speed}, + gpio::{AnyPin, Level, Output, Speed}, mode::Async, spi::{self, MisoPin, MosiPin, SckPin}, - time::hz, - Peripheral, + time::hz, Peri, }; pub const SPI_MIN_BUF_LEN: usize = 14; @@ -251,13 +250,13 @@ impl<'a, 'buf> Bmi323<'a, 'buf> { ///t creates a new BMI085 instance from uninitialized pins pub fn new_from_pins( - peri: impl Peripheral

+ 'a, - sck_pin: impl Peripheral

> + 'a, - mosi_pin: impl Peripheral

> + 'a, - miso_pin: impl Peripheral

> + 'a, - tx_dma: impl Peripheral

> + 'a, - rx_dma: impl Peripheral

> + 'a, - spi_cs_pin: impl Pin, + peri: Peri<'a, SpiPeri>, + sck_pin: Peri<'a, impl SckPin>, + mosi_pin: Peri<'a, impl MosiPin>, + miso_pin: Peri<'a, impl MisoPin>, + tx_dma: Peri<'a, impl spi::TxDma>, + rx_dma: Peri<'a, impl spi::RxDma>, + spi_cs_pin: Peri<'a, AnyPin>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { let mut spi_config = spi::Config::default(); diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index 20ad800f..124f8eeb 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -3,8 +3,7 @@ use core::ops::Range; use embassy_stm32::{ mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, - time::mhz, - Peripheral, + time::mhz, Peri, }; use smart_leds::RGB8; @@ -58,10 +57,10 @@ where } pub fn new_from_pins( - peri: impl Peripheral

+ 'a, - sck_pin: impl Peripheral

> + 'a, - mosi_pin: impl Peripheral

> + 'a, - tx_dma: impl Peripheral

> + 'a, + peri: Peri<'static, SpiPeri>, + sck_pin: Peri<'static, impl SckPin>, + mosi_pin: Peri<'static, impl MosiPin>, + tx_dma: Peri<'static, impl spi::TxDma>, spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], ) -> Self { let mut dotstar_spi_config = Config::default(); diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 3c680a04..5729fc3e 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -1,5 +1,4 @@ -use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime}; -use embassy_stm32::Peripheral; +use embassy_stm32::{adc::{self, Adc, AdcChannel, Resolution, SampleTime}, Peri}; // The voltage which the internal ADC were calibrated at. // For the H743 and F407 @@ -15,7 +14,7 @@ impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> { // NOTE: vref_int_peri is not checked by compiler and needs to // be the peripheral connected to Vref_int. pub fn new( - peri: impl Peripheral

+ 'a, + peri: Peri<'static, T>, pin: Ch, sample_time: SampleTime, resolution: Resolution, diff --git a/lib-stm32/src/drivers/switches/button.rs b/lib-stm32/src/drivers/switches/button.rs index ea315559..f7d2495f 100644 --- a/lib-stm32/src/drivers/switches/button.rs +++ b/lib-stm32/src/drivers/switches/button.rs @@ -2,8 +2,7 @@ use defmt::Format; use embassy_futures::select; use embassy_stm32::{ exti::ExtiInput, - gpio::{Pin, Pull}, - Peripheral, + gpio::{Pin, Pull}, Peri, }; use embassy_time::{Instant, Timer}; @@ -59,8 +58,8 @@ impl< } pub fn new_from_pins( - input_pin: PIN, - input_pin_exti: impl Peripheral

::ExtiChannel> + 'static, + input_pin: Peri<'static, PIN>, + input_pin_exti: Peri<'static, ::ExtiChannel>, input_inverted: bool, ) -> Self { let input = ExtiInput::new(input_pin, input_pin_exti, Pull::None); diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs index 66416ac0..2e1ab1f1 100644 --- a/lib-stm32/src/drivers/switches/dip.rs +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -3,7 +3,7 @@ use core::{ ops::Range, }; -use embassy_stm32::gpio::{AnyPin, Input, Pull}; +use embassy_stm32::{gpio::{AnyPin, Input, Pull}, Peri}; pub struct DipSwitch<'a, const PIN_CT: usize> { inputs: [Input<'a>; PIN_CT], @@ -28,7 +28,7 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { } pub fn new_from_pins( - pins: [AnyPin; PIN_CT], + pins: [Peri<'static, AnyPin>; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>, ) -> DipSwitch<'a, PIN_CT> { diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs index 76b652a5..56ed79b0 100644 --- a/lib-stm32/src/drivers/switches/rotary_encoder.rs +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -1,4 +1,4 @@ -use embassy_stm32::gpio::{AnyPin, Input, Pull}; +use embassy_stm32::{gpio::{AnyPin, Input, Pull}, Peri}; pub struct RotaryEncoder<'a, const PIN_CT: usize> { pins: [Input<'a>; PIN_CT], @@ -26,7 +26,7 @@ impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { } pub fn new_from_pins( - pins: [AnyPin; PIN_CT], + pins: [Peri<'static, AnyPin>; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>, ) -> RotaryEncoder<'a, PIN_CT> { diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 13aff49b..b990db05 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -3,7 +3,6 @@ #![allow(clippy::too_many_arguments)] // too many functions passing pins to device drivers exceed the bound #![feature(generic_const_exprs)] #![feature(const_precise_live_drops)] -#![feature(maybe_uninit_uninit_array)] #![feature(sync_unsafe_cell)] #![feature(type_alias_impl_trait)] #![feature(impl_trait_in_assoc_type)] diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 80dff948..13945996 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -194,6 +194,7 @@ impl< &self.uart_read_queue } + #[define_opaque(IdleBufferedUartReadFuture)] pub fn read_task( &'static self, rx: UartRx<'static, Async>, @@ -217,6 +218,7 @@ impl< &self.uart_write_queue } + #[define_opaque(IdleBufferedUartWriteFuture)] pub fn write_task( &'static self, tx: UartTx<'static, Async>, @@ -466,6 +468,7 @@ impl Self { queue_rx: queue } } + #[define_opaque(ReadTaskFuture)] fn read_task( &'static self, // queue_rx: &'static Queue, @@ -623,6 +626,7 @@ impl Self { queue_tx: queue } } + #[define_opaque(WriteTaskFuture)] fn write_task( &'static self, mut tx: UartTx<'static, Async>, diff --git a/power-board/Cargo.lock b/power-board/Cargo.lock index 6d4c7a79..208254d3 100644 --- a/power-board/Cargo.lock +++ b/power-board/Cargo.lock @@ -42,14 +42,14 @@ name = "ateam-lib-stm32" version = "0.1.0" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "embassy-executor", "embassy-futures", "embassy-stm32", "embassy-sync", "embassy-time", - "heapless", + "heapless 0.9.1", "num-traits", "paste", "smart-leds", @@ -65,7 +65,7 @@ dependencies = [ "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "defmt-rtt", "defmt-test", "embassy-executor", @@ -91,9 +91,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bare-metal" @@ -147,9 +147,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.8.0" +version = "2.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36" +checksum = "34efbcccd345379ca2868b2b2c9d3782e9cc58ba87bc7d79d5b53d9c9ae6f25d" [[package]] name = "block-device-driver" @@ -162,9 +162,9 @@ dependencies = [ [[package]] name = "bytemuck" -version = "1.21.0" +version = "1.23.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" +checksum = "3995eaeebcdf32f91f980d360f78732ddc061097ab4e39991ae7a6ace9194677" [[package]] name = "byteorder" @@ -183,9 +183,9 @@ dependencies = [ [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9" [[package]] name = "clang-sys" @@ -292,9 +292,9 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "darling" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" dependencies = [ "darling_core", "darling_macro", @@ -302,9 +302,9 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", @@ -316,9 +316,9 @@ dependencies = [ [[package]] name = "darling_macro" -version = "0.20.10" +version = "0.20.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", "quote", @@ -327,9 +327,18 @@ dependencies = [ [[package]] name = "defmt" -version = "0.3.10" +version = "0.3.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -337,9 +346,9 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.4.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", @@ -350,40 +359,40 @@ dependencies = [ [[package]] name = "defmt-parser" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", ] [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "24076cc7203c365e7febfcec15d6667a9ef780bd2c5fd3b2a197400df78f299b" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", - "defmt", + "defmt 1.0.1", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", @@ -392,9 +401,9 @@ dependencies = [ [[package]] name = "document-features" -version = "0.2.10" +version = "0.2.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb6969eaabd2421f8a2775cfd2471a2b634372b4a25d41e3bd647b79912850a0" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" dependencies = [ "litrs", ] @@ -407,10 +416,10 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", "embassy-futures", "embassy-hal-internal", "embassy-sync", @@ -425,20 +434,20 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.8.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-executor-macros", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.7.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "darling", "proc-macro2", @@ -449,41 +458,41 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" [[package]] name = "embassy-hal-internal" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cortex-m", "critical-section", - "defmt", + "defmt 1.0.1", "num-traits", ] [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "aligned", "bit_field", - "bitflags 2.8.0", + "bitflags 2.9.3", "block-device-driver", "cfg-if", "cortex-m", "cortex-m-rt", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-embedded-hal", "embassy-futures", @@ -508,7 +517,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.3", "sdio-host", "static_assertions", "stm32-fmc", @@ -519,66 +529,67 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.7.1" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", - "heapless", + "heapless 0.8.0", ] [[package]] name = "embassy-time" version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "cfg-if", "critical-section", - "defmt", + "defmt 1.0.1", "document-features", "embassy-time-driver", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", - "futures-util", + "futures-core", ] [[package]] name = "embassy-time-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "embassy-executor", - "heapless", + "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ - "defmt", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bcebe4c4d5b597da0b8741916e450c46e6fef06e" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" dependencies = [ "critical-section", - "defmt", + "defmt 1.0.1", "embassy-sync", "embassy-usb-driver", ] @@ -633,7 +644,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ - "defmt", + "defmt 0.3.100", ] [[package]] @@ -642,7 +653,7 @@ version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" dependencies = [ - "defmt", + "defmt 0.3.100", "embedded-io", ] @@ -676,12 +687,12 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.11" +version = "0.3.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "976dd42dc7e85965fe702eb8164f21f450704bdde31faefd6471dba214cb594e" +checksum = "778e2ac28f6c47af28e4907f13ffd1e1ddbd400980a9abd7c8df189bf578a5ad" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -722,9 +733,9 @@ dependencies = [ [[package]] name = "glob" -version = "0.3.2" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2" +checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280" [[package]] name = "hash32" @@ -751,6 +762,16 @@ dependencies = [ "stable_deref_trait", ] +[[package]] +name = "heapless" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5" +dependencies = [ + "hash32", + "stable_deref_trait", +] + [[package]] name = "hermit-abi" version = "0.1.19" @@ -766,7 +787,7 @@ version = "0.5.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589533453244b0995c858700322199b2becb13b627df2851f64a2775d024abcf" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -805,18 +826,18 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.172" +version = "0.2.175" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa" +checksum = "6a82ae493e598baaea5209805c49bbf2ea7de956d50d7da0da1164f9c6d28543" [[package]] name = "libloading" -version = "0.8.6" +version = "0.8.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc2f4eb4bc735547cfed7c0a4922cbd04a4655978c09b54f1f7b228750664c34" +checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667" dependencies = [ "cfg-if", - "windows-targets", + "windows-targets 0.53.3", ] [[package]] @@ -827,9 +848,9 @@ checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" [[package]] name = "litrs" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +checksum = "f5e54036fe321fd421e10d732f155734c4e4afd610dd556d9a82833ab3ee0bed" [[package]] name = "log" @@ -839,9 +860,9 @@ checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "memchr" -version = "2.7.4" +version = "2.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" +checksum = "32a282da65faaf38286cf3be983213fcf1d2e2a58700e808f83f4ea9a4804bc0" [[package]] name = "minimal-lexical" @@ -897,12 +918,12 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +checksum = "fd402d00b0fb94c5aee000029204a46884b1262e0c443f166d86d2c0747e1a1a" dependencies = [ "cortex-m", - "defmt", + "defmt 1.0.1", ] [[package]] @@ -953,18 +974,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.93" +version = "1.0.101" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.38" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -975,11 +996,17 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "99d9a13982dcf210057a8a78572b2217b667c3beacbf3a0d8b454f6f82837d38" + [[package]] name = "regex" -version = "1.11.1" +version = "1.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191" +checksum = "23d7fd106d8c02486a8d64e778353d1cffe08ce79ac2e82f540c86d0facf6912" dependencies = [ "aho-corasick", "memchr", @@ -989,9 +1016,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.9" +version = "0.4.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908" +checksum = "6b9458fa0bfeeac22b5ca447c63aaf45f28439a709ccd244698632f9aa6394d6" dependencies = [ "aho-corasick", "memchr", @@ -1000,15 +1027,15 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.5" +version = "0.8.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c" +checksum = "caf4aa5b0f434c91fe5c7f1ecb6a5ece2130b02ad2a590589dda5146df959001" [[package]] name = "rgb" -version = "0.8.50" +version = "0.8.52" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57397d16646700483b67d2dd6511d79318f9d057fdbd21a4066aeac8b41d310a" +checksum = "0c6a884d2998352bb4daf0183589aec883f16a6da1f4dde84d8e2e9a5409a1ce" dependencies = [ "bytemuck", ] @@ -1034,18 +1061,18 @@ version = "0.38.44" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fdb5bc1ae2baa591800df16c9ca78619bf65c0488b41b96ccec5d11220d8c154" dependencies = [ - "bitflags 2.8.0", + "bitflags 2.9.3", "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.59.0", ] [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -1109,12 +1136,12 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +version = "17.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] @@ -1131,9 +1158,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" -version = "2.0.98" +version = "2.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "36147f1a48ae0ec2b5b3bc5b537d267457555a10dc06f3dbc8cb11ba3006d3b1" +checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6" dependencies = [ "proc-macro2", "quote", @@ -1157,18 +1184,18 @@ checksum = "c13547615a44dc9c452a8a534638acdf07120d4b6847c8178705da06306a3057" [[package]] name = "thiserror" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d452f284b73e6d76dd36758a0c8684b1d5be31f92b89d07fd5822175732206fc" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.11" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26afc1baea8a989337eeb52b6e72a039780ce45c3edfcc9c5b9d112feeb173c2" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -1177,9 +1204,9 @@ dependencies = [ [[package]] name = "unicode-ident" -version = "1.0.16" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a210d160f08b701c8721ba1c726c11662f877ea6b7094007e1ca9a1041945034" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "unicode-xid" @@ -1238,11 +1265,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.9" +version = "0.1.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +checksum = "0978bf7171b3d90bac376700cb56d606feb40f251a475a5d6634613564460b22" dependencies = [ - "windows-sys", + "windows-sys 0.60.2", ] [[package]] @@ -1251,13 +1278,28 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" +[[package]] +name = "windows-link" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a" + [[package]] name = "windows-sys" version = "0.59.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" dependencies = [ - "windows-targets", + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-sys" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2f500e4d28234f72040990ec9d39e3a6b950f9f22d3dba18416c35882612bcb" +dependencies = [ + "windows-targets 0.53.3", ] [[package]] @@ -1266,14 +1308,31 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm 0.52.6", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", +] + +[[package]] +name = "windows-targets" +version = "0.53.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5fe6031c4041849d7c496a8ded650796e7b6ecc19df1a431c1a363342e5dc91" +dependencies = [ + "windows-link", + "windows_aarch64_gnullvm 0.53.0", + "windows_aarch64_msvc 0.53.0", + "windows_i686_gnu 0.53.0", + "windows_i686_gnullvm 0.53.0", + "windows_i686_msvc 0.53.0", + "windows_x86_64_gnu 0.53.0", + "windows_x86_64_gnullvm 0.53.0", + "windows_x86_64_msvc 0.53.0", ] [[package]] @@ -1282,44 +1341,92 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764" + [[package]] name = "windows_aarch64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" +[[package]] +name = "windows_aarch64_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c" + [[package]] name = "windows_i686_gnu" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" +[[package]] +name = "windows_i686_gnu" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3" + [[package]] name = "windows_i686_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" +[[package]] +name = "windows_i686_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11" + [[package]] name = "windows_i686_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" +[[package]] +name = "windows_i686_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d" + [[package]] name = "windows_x86_64_gnu" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" +[[package]] +name = "windows_x86_64_gnu" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba" + [[package]] name = "windows_x86_64_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57" + [[package]] name = "windows_x86_64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.53.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" diff --git a/power-board/Cargo.toml b/power-board/Cargo.toml index cd3b0a4a..88ce7f8c 100644 --- a/power-board/Cargo.toml +++ b/power-board/Cargo.toml @@ -9,7 +9,7 @@ repository = "https://github.com/SSL-A-Team/firmware" cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.7.0", features = [ +embassy-executor = { version = "0.8.0", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", @@ -20,7 +20,7 @@ embassy-time = { version = "0.4.0", features = [ "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.2.0", features = [ +embassy-stm32 = { version = "0.3.0", features = [ "defmt", "stm32g030c8", "unstable-pac", @@ -28,13 +28,13 @@ embassy-stm32 = { version = "0.2.0", features = [ "exti", ] } embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.6.2" } +embassy-sync = { version = "0.7.1" } -defmt = "0.3.10" -defmt-rtt = "0.4.1" +defmt = "1.0.1" +defmt-rtt = "1.0.0" futures-util = { version = "0.3.31", default-features = false } -panic-probe = { version = "0.3", features = ["print-defmt"] } +panic-probe = { version = "1.0.0", features = ["print-defmt"] } critical-section = "1.2.0" const_format = "0.2.34" @@ -44,7 +44,7 @@ ateam-common-packets = { path = "../software-communication/ateam-common-packets/ smart-leds = "0.4.0" [dev-dependencies] -defmt-test = "0.3.0" +defmt-test = "0.4.0" [profile.dev] opt-level = 3 diff --git a/power-board/src/bin/hwtest-bringup/main.rs b/power-board/src/bin/hwtest-bringup/main.rs index c97888a8..c2737be8 100644 --- a/power-board/src/bin/hwtest-bringup/main.rs +++ b/power-board/src/bin/hwtest-bringup/main.rs @@ -80,7 +80,7 @@ async fn main(_spawner: Spawner) { let mut adc_buf: [u16; 7] = [0; 7]; // p.PC6 - Buzzer io pin - let ch1 = PwmPin::new_ch1(p.PC6, OutputType::PushPull); + let ch1 = PwmPin::new(p.PC6, OutputType::PushPull); // p.TIM3 - Buzzer timer let pwm = SimplePwm::new( p.TIM3, @@ -97,7 +97,7 @@ async fn main(_spawner: Spawner) { loop { adc.read( - &mut adc_dma, + adc_dma.reborrow(), [ (&mut cell0_adc_pin, SampleTime::CYCLES160_5), (&mut cell1_adc_pin, SampleTime::CYCLES160_5), diff --git a/power-board/src/tasks/audio_task.rs b/power-board/src/tasks/audio_task.rs index cf01b375..635c9c2f 100644 --- a/power-board/src/tasks/audio_task.rs +++ b/power-board/src/tasks/audio_task.rs @@ -10,7 +10,7 @@ use embassy_stm32::{ timer::{ simple_pwm::{PwmPin, SimplePwm}, Channel, - }, + }, Peri, }; use embassy_time::{Duration, Ticker, Timer}; @@ -104,10 +104,10 @@ async fn audio_task_entry( pub fn start_audio_task( spawner: &Spawner, audio_subscriber: AudioSubscriber, - buzzer_pin: BuzzerPwmPin, - buzzer_timer: BuzzerTimer, + buzzer_pin: Peri<'static, BuzzerPwmPin>, + buzzer_timer: Peri<'static, BuzzerTimer>, ) { - let ch1 = PwmPin::new_ch1(buzzer_pin, OutputType::PushPull); + let ch1 = PwmPin::new(buzzer_pin, OutputType::PushPull); let pwm = SimplePwm::new( buzzer_timer, Some(ch1), diff --git a/power-board/src/tasks/coms_task.rs b/power-board/src/tasks/coms_task.rs index db9e7531..2e42a3c9 100644 --- a/power-board/src/tasks/coms_task.rs +++ b/power-board/src/tasks/coms_task.rs @@ -1,6 +1,6 @@ use core::mem::MaybeUninit; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::usart::{self, DataBits, Parity, StopBits, Uart}; +use embassy_stm32::{usart::{self, DataBits, Parity, StopBits, Uart}, Peri}; use crate::{pins::*, power_state::SharedPowerState, SystemIrqs, DEBUG_UART_QUEUES}; use ateam_common_packets::bindings::{BatteryInfo, PowerCommand, PowerTelemetry}; @@ -191,11 +191,11 @@ pub async fn start_coms_task( shared_power_state: &'static SharedPowerState, telemetry_subscriber: TelemetrySubscriber, audio_publisher: AudioPublisher, - uart: ComsUart, - uart_rx_pin: ComsUartRxPin, - uart_tx_pin: ComsUartTxPin, - uart_rx_dma: ComsDmaRx, - uart_tx_dma: ComsDmaTx, + uart: Peri<'static, ComsUart>, + uart_rx_pin: Peri<'static, ComsUartRxPin>, + uart_tx_pin: Peri<'static, ComsUartTxPin>, + uart_rx_dma: Peri<'static, ComsDmaRx>, + uart_tx_dma: Peri<'static, ComsDmaTx>, ) { let uart_config = power_uart_config(); let coms_uart = Uart::new( diff --git a/power-board/src/tasks/power_task.rs b/power-board/src/tasks/power_task.rs index 7159a4a3..775e9262 100644 --- a/power-board/src/tasks/power_task.rs +++ b/power-board/src/tasks/power_task.rs @@ -13,7 +13,7 @@ use ateam_lib_stm32::{ use embassy_executor::Spawner; use embassy_stm32::{ adc::{Adc, AdcChannel, AnyAdcChannel, SampleTime}, - peripherals::ADC1, + peripherals::ADC1, Peri, }; use embassy_time::{Duration, Instant, Ticker, Timer}; @@ -60,19 +60,19 @@ async fn power_task_entry( shared_power_state: &'static SharedPowerState, telemetry_publisher: TelemetryPublisher, audio_publisher: AudioPublisher, - adc: PowerAdc, - mut adc_dma: PowerAdcDma, - cell1_adc_pin: BatteryCell1VoltageMonitorPin, - cell2_adc_pin: BatteryCell2VoltageMonitorPin, - cell3_adc_pin: BatteryCell3VoltageMonitorPin, - cell4_adc_pin: BatteryCell4VoltageMonitorPin, - cell5_adc_pin: BatteryCell5VoltageMonitorPin, - cell6_adc_pin: BatteryCell6VoltageMonitorPin, - power_rail_12v0_adc_pin: Power12v0VoltageMonitorPin, - power_rail_5v0_adc_pin: Power5v0VoltageMonitorPin, - power_rail_3v3_adc_pin: Power3v3VoltageMonitorPin, - power_rail_vbatt_before_lsw_adc_pin: BatteryPreLoadSwitchVoltageMonitorPin, - power_rail_vbatt_adc_pin: BatteryVoltageMonitorPin, + adc: Peri<'static, PowerAdc>, + mut adc_dma: Peri<'static, PowerAdcDma>, + cell1_adc_pin: Peri<'static, BatteryCell1VoltageMonitorPin>, + cell2_adc_pin: Peri<'static, BatteryCell2VoltageMonitorPin>, + cell3_adc_pin: Peri<'static, BatteryCell3VoltageMonitorPin>, + cell4_adc_pin: Peri<'static, BatteryCell4VoltageMonitorPin>, + cell5_adc_pin: Peri<'static, BatteryCell5VoltageMonitorPin>, + cell6_adc_pin: Peri<'static, BatteryCell6VoltageMonitorPin>, + power_rail_12v0_adc_pin: Peri<'static, Power12v0VoltageMonitorPin>, + power_rail_5v0_adc_pin: Peri<'static, Power5v0VoltageMonitorPin>, + power_rail_3v3_adc_pin: Peri<'static, Power3v3VoltageMonitorPin>, + power_rail_vbatt_before_lsw_adc_pin: Peri<'static, BatteryPreLoadSwitchVoltageMonitorPin>, + power_rail_vbatt_adc_pin: Peri<'static, BatteryVoltageMonitorPin>, ) { // give time for the power on sequence to happen Timer::after_millis(500).await; @@ -174,7 +174,7 @@ async fn power_task_entry( ] .into_iter(); adc.read( - &mut adc_dma, + adc_dma.reborrow(), power_rail_read_seq, &mut power_rail_adc_raw_samples, ) @@ -241,7 +241,7 @@ async fn power_task_entry( ] .into_iter(); adc.read( - &mut adc_dma, + adc_dma.reborrow(), battery_cell_read_seq, &mut battery_cell_adc_raw_samples, ) @@ -395,19 +395,19 @@ pub async fn start_power_task( shared_power_state: &'static SharedPowerState, telemetry_publisher: TelemetryPublisher, audio_publisher: AudioPublisher, - adc: PowerAdc, - adc_dma: PowerAdcDma, - cell1_adc_pin: BatteryCell1VoltageMonitorPin, - cell2_adc_pin: BatteryCell2VoltageMonitorPin, - cell3_adc_pin: BatteryCell3VoltageMonitorPin, - cell4_adc_pin: BatteryCell4VoltageMonitorPin, - cell5_adc_pin: BatteryCell5VoltageMonitorPin, - cell6_adc_pin: BatteryCell6VoltageMonitorPin, - power_rail_12v0_adc_pin: Power12v0VoltageMonitorPin, - power_rail_5v0_adc_pin: Power5v0VoltageMonitorPin, - power_rail_3v3_adc_pin: Power3v3VoltageMonitorPin, - power_rail_vbatt_before_lsw_adc_pin: BatteryPreLoadSwitchVoltageMonitorPin, - power_rail_vbatt_adc_pin: BatteryVoltageMonitorPin, + adc: Peri<'static, PowerAdc>, + adc_dma: Peri<'static, PowerAdcDma>, + cell1_adc_pin: Peri<'static, BatteryCell1VoltageMonitorPin>, + cell2_adc_pin: Peri<'static, BatteryCell2VoltageMonitorPin>, + cell3_adc_pin: Peri<'static, BatteryCell3VoltageMonitorPin>, + cell4_adc_pin: Peri<'static, BatteryCell4VoltageMonitorPin>, + cell5_adc_pin: Peri<'static, BatteryCell5VoltageMonitorPin>, + cell6_adc_pin: Peri<'static, BatteryCell6VoltageMonitorPin>, + power_rail_12v0_adc_pin: Peri<'static, Power12v0VoltageMonitorPin>, + power_rail_5v0_adc_pin: Peri<'static, Power5v0VoltageMonitorPin>, + power_rail_3v3_adc_pin: Peri<'static, Power3v3VoltageMonitorPin>, + power_rail_vbatt_before_lsw_adc_pin: Peri<'static, BatteryPreLoadSwitchVoltageMonitorPin>, + power_rail_vbatt_adc_pin: Peri<'static, BatteryVoltageMonitorPin>, ) { spawner .spawn(power_task_entry( From bb9ba64499fd92512e1fc93cda584c62c1a4d779 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 24 Aug 2025 20:55:56 -0400 Subject: [PATCH 10/22] fix kicker telemetry race condition --- control-board/src/tasks/kicker_task.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index ea9f495c..f5218633 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -209,6 +209,8 @@ impl< // Rate limit the retry loop // Timer::after_millis(10).await; } + + main_loop_ticker.reset(); } KickerTaskState::Reset => { defmt::trace!( @@ -219,7 +221,7 @@ impl< // Reset the connection timeout period connection_timeout_start = Instant::now(); // Move on to ConnectUart state - self.kicker_task_state = KickerTaskState::ConnectUart; + self.kicker_task_state = KickerTaskState::Connected; } KickerTaskState::ConnectUart => { if telemetry_received { From 2caee68a816e4b2f678238c9c0dce6a5d64bdb46 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 17:49:39 -0400 Subject: [PATCH 11/22] resolve some warning, update CI deps --- .github/workflows/CI.yml | 9 +++++---- lib-stm32/src/drivers/boot/stm32_interface.rs | 8 +++----- lib-stm32/src/drivers/radio/edm_protocol.rs | 4 ++-- lib-stm32/src/queue.rs | 8 ++++---- lib-stm32/src/uart/queue.rs | 2 +- 5 files changed, 15 insertions(+), 16 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 5fa1848b..a3e890be 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -9,20 +9,21 @@ jobs: test-firmware-build: runs-on: ubuntu-24.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v5 - name: Fetch Submodules - uses: webfactory/ssh-agent@v0.7.0 + uses: webfactory/ssh-agent@v0.9.1 with: ssh-private-key: | ${{ secrets.ROBOT_PRIVATE_DATA_PRIVATE_SSH_KEY }} - run: | git submodule update --init --recursive ./util/ateam-credentials-setup.bash - - uses: cachix/install-nix-action@v16 + - uses: cachix/install-nix-action@v31 with: extra_nix_config: access-tokens = github.com=${{ secrets.GITHUB_TOKEN }} - + trusted-public-keys = cache.nixos.org-1:6NCHdD59X431o0gWypbMrAURkbJ16ZPMQFGspcDShjY= mycache.example.org:somePublicKeyString= + substituters = https://cache.nixos.org https://mycache.example.org - name: Validate Nix Environment shell: bash run: | diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index 34500efa..c4c1ddd7 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -229,7 +229,7 @@ impl< } } - pub fn try_read_data(&self) -> Result, Error> { + pub fn try_read_data(&'_ self) -> Result, Error> { return self.reader.try_dequeue(); } @@ -600,10 +600,8 @@ impl< } pub async fn execute_code(&mut self, start_address: Option) -> Result<(), ()> { - defmt::debug!("firmware jump/go command implementation not working. will reset part."); - self.reset_into_program(false).await; - return Ok(()); - + defmt::warn!("firmware jump/go command implementation does not work on some parts. If this appears not to work, consider simply resetting the device."); + if !self.in_bootloader { defmt::error!("called bootloader operation when not in bootloader context."); return Err(()); diff --git a/lib-stm32/src/drivers/radio/edm_protocol.rs b/lib-stm32/src/drivers/radio/edm_protocol.rs index 60198a94..979dc2f1 100644 --- a/lib-stm32/src/drivers/radio/edm_protocol.rs +++ b/lib-stm32/src/drivers/radio/edm_protocol.rs @@ -103,7 +103,7 @@ impl EdmPacketRaw { Ok(total_len) } - fn get_payload(&self) -> Result { + fn get_payload(&'_ self) -> Result, EdmPacketError> { match u16::from_be(self.identifier) { Self::CONNECT_EVENT => { let event = data_to_ref::>(&self.payload)?; @@ -176,7 +176,7 @@ pub enum EdmPacket<'a> { } impl EdmPacket<'_> { - pub fn new(data: &[u8]) -> Result { + pub fn new(data: &'_ [u8]) -> Result, EdmPacketError> { EdmPacketRaw::new(data)?.get_payload() } diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index ba50a1c4..3d9f570d 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -122,7 +122,7 @@ impl Queue { !self.read_in_progress.load(Ordering::Relaxed) && self.size.load(Ordering::Relaxed) > 0 } - pub fn try_dequeue(&self) -> Result, Error> { + pub fn try_dequeue(&'_ self) -> Result, Error> { critical_section::with(|_| { if self.read_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); @@ -152,7 +152,7 @@ impl Queue { }) } - pub async fn dequeue(&self) -> Result, Error> { + pub async fn dequeue(&'_ self) -> Result, Error> { // TODO: look at this (when uncommented causes issue cancelling dequeue) // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { // return Err(Error::InProgress); @@ -204,7 +204,7 @@ impl Queue { }); } - pub fn try_enqueue(&self) -> Result, Error> { + pub fn try_enqueue(&'_ self) -> Result, Error> { critical_section::with(|_| { if self.write_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); @@ -242,7 +242,7 @@ impl Queue { }) } - pub fn try_enqueue_override(&self) -> Result, Error> { + pub fn try_enqueue_override(&'_ self) -> Result, Error> { critical_section::with(|_| { if self.write_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 13945996..6227a7bd 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -605,7 +605,7 @@ impl self.queue_rx.can_dequeue() } - pub fn try_dequeue(&self) -> Result, Error> { + pub fn try_dequeue(&'_ self) -> Result, Error> { self.queue_rx.try_dequeue() } From 6b9f57e2741717dffeb3b78d0aed2996cfc6c4f6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 18:11:46 -0400 Subject: [PATCH 12/22] add warning promotion to lint check for library --- .github/workflows/rustfmt.yml | 2 +- .github/workflows/rustlint.yml | 35 ++++++++++++++++++++++++++++++++++ lib-stm32/Cargo.toml | 1 + lib-stm32/src/lib.rs | 5 +++++ 4 files changed, 42 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/rustlint.yml diff --git a/.github/workflows/rustfmt.yml b/.github/workflows/rustfmt.yml index 6d43ba02..e7855ed4 100644 --- a/.github/workflows/rustfmt.yml +++ b/.github/workflows/rustfmt.yml @@ -12,7 +12,7 @@ steps: - uses: actions/checkout@v4 - name: Fetch Submodules - uses: webfactory/ssh-agent@v0.7.0 + uses: webfactory/ssh-agent@v0.9.1 with: ssh-private-key: | ${{ secrets.ROBOT_PRIVATE_DATA_PRIVATE_SSH_KEY }} diff --git a/.github/workflows/rustlint.yml b/.github/workflows/rustlint.yml new file mode 100644 index 00000000..ef223415 --- /dev/null +++ b/.github/workflows/rustlint.yml @@ -0,0 +1,35 @@ +name: A-Team Firmware +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] +jobs: + test-firmware-build: + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v5 + - name: Fetch Submodules + uses: webfactory/ssh-agent@v0.9.1 + with: + ssh-private-key: | + ${{ secrets.ROBOT_PRIVATE_DATA_PRIVATE_SSH_KEY }} + - run: | + git submodule update --init --recursive + ./util/ateam-credentials-setup.bash + - uses: cachix/install-nix-action@v31 + with: + extra_nix_config: + access-tokens = github.com=${{ secrets.GITHUB_TOKEN }} + trusted-public-keys = cache.nixos.org-1:6NCHdD59X431o0gWypbMrAURkbJ16ZPMQFGspcDShjY= mycache.example.org:somePublicKeyString= + substituters = https://cache.nixos.org https://mycache.example.org + - name: Validate Nix Environment + shell: bash + run: | + nix flake check + nix develop -c cargo --version + nix develop -c arm-none-eabi-gcc --version + - name: Lint Common Libraries + shell: bash + run: | + nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 0f7e2c55..320b5eff 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -25,6 +25,7 @@ smart-leds = "0.4.0" [features] default = ["embassy-stm32/stm32h723zg"] +strict = ["embassy-stm32/stm32h723zg"] # stm32h723zg = [] [patch.crates-io] diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index b990db05..23775ea6 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -1,6 +1,11 @@ #![no_std] + #![allow(incomplete_features)] #![allow(clippy::too_many_arguments)] // too many functions passing pins to device drivers exceed the bound + +// if "strict" feature is on, promote warnings to errors +#![cfg_attr(feature = "strict", deny(warnings))] + #![feature(generic_const_exprs)] #![feature(const_precise_live_drops)] #![feature(sync_unsafe_cell)] From daa11883fc50ad4093ff04862be815f74aa3eb04 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 18:16:12 -0400 Subject: [PATCH 13/22] fix link CI name, lib-stm32 should be warning free --- .github/workflows/CI.yml | 4 ++-- .github/workflows/rustlint.yml | 12 ++++++++---- lib-stm32/src/queue.rs | 2 +- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index a3e890be..a2dac17a 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -1,4 +1,4 @@ -name: A-Team Firmware +name: A-Team Firmware Compile on: push: branches: [ main ] @@ -7,7 +7,7 @@ on: - '**' jobs: test-firmware-build: - runs-on: ubuntu-24.04 + runs-on: ubuntu-latest steps: - uses: actions/checkout@v5 - name: Fetch Submodules diff --git a/.github/workflows/rustlint.yml b/.github/workflows/rustlint.yml index ef223415..45eb69e8 100644 --- a/.github/workflows/rustlint.yml +++ b/.github/workflows/rustlint.yml @@ -1,12 +1,12 @@ -name: A-Team Firmware +name: Rust Linter Checks on: push: branches: [ main ] pull_request: branches: [ main ] jobs: - test-firmware-build: - runs-on: ubuntu-24.04 + lint: + runs-on: ubuntu-latest steps: - uses: actions/checkout@v5 - name: Fetch Submodules @@ -29,7 +29,11 @@ jobs: nix flake check nix develop -c cargo --version nix develop -c arm-none-eabi-gcc --version - - name: Lint Common Libraries + - name: Common Libraries No Warnings + shell: bash + run: | + nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml + - name: Common Libraries No Warnings shell: bash run: | nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index 3d9f570d..9f28de0b 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -304,7 +304,7 @@ impl Queue { }) } - pub async fn enqueue(&self) -> Result, Error> { + pub async fn enqueue(&'_ self) -> Result, Error> { /* Safety: this raw pointer access is safe because the underlying memory is statically allocated * and the total read operation is atomic across tasks because of the critical_section closure */ From 43429af3e02afc105938aaf4393ce096c3b065fd Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 18:23:28 -0400 Subject: [PATCH 14/22] add clippy check --- .github/workflows/rustlint.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/rustlint.yml b/.github/workflows/rustlint.yml index 45eb69e8..2e0948d9 100644 --- a/.github/workflows/rustlint.yml +++ b/.github/workflows/rustlint.yml @@ -37,3 +37,7 @@ jobs: shell: bash run: | nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml + - name: Common Libraries Clippy Default + shell: bash + run: | + nix develop -c cargo clippy --all-targets --all-features --manifest-path lib-stm32/Cargo.toml From f1a341373518986766044f70db6be6b9e0e19397 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 18:25:08 -0400 Subject: [PATCH 15/22] remove duplicate warning check --- .github/workflows/rustlint.yml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.github/workflows/rustlint.yml b/.github/workflows/rustlint.yml index 2e0948d9..7a431c94 100644 --- a/.github/workflows/rustlint.yml +++ b/.github/workflows/rustlint.yml @@ -33,10 +33,6 @@ jobs: shell: bash run: | nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml - - name: Common Libraries No Warnings - shell: bash - run: | - nix develop -c cargo build --features strict --manifest-path lib-stm32/Cargo.toml - name: Common Libraries Clippy Default shell: bash run: | From 5ed347557f68e17d8fce4e28b8947f45a7754ec2 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 20:33:01 -0400 Subject: [PATCH 16/22] pass clippy --- lib-stm32/src/drivers/audio/buzzer.rs | 2 +- lib-stm32/src/drivers/boot/stm32_interface.rs | 48 +++++++------------ lib-stm32/src/drivers/radio/odin_w26x.rs | 9 ++-- lib-stm32/src/power/battery.rs | 8 ++-- lib-stm32/src/queue.rs | 2 +- lib-stm32/src/time/mod.rs | 9 +++- lib-stm32/src/uart/queue.rs | 8 ++-- 7 files changed, 40 insertions(+), 46 deletions(-) diff --git a/lib-stm32/src/drivers/audio/buzzer.rs b/lib-stm32/src/drivers/audio/buzzer.rs index ff655384..f916b789 100644 --- a/lib-stm32/src/drivers/audio/buzzer.rs +++ b/lib-stm32/src/drivers/audio/buzzer.rs @@ -33,7 +33,7 @@ impl<'d, T: GeneralInstance4Channel> Buzzer<'d, T> { ) -> Self { Self { pwm, - channel: channel, + channel, min_freq, max_freq, } diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index c4c1ddd7..b3509d6d 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -182,7 +182,7 @@ impl< let sync_res = with_timeout( Duration::from_millis(5000), self.reader.read(|buf| { - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { defmt::debug!("bootloader replied with ACK after calibration."); self.in_bootloader = true; @@ -230,11 +230,11 @@ impl< } pub fn try_read_data(&'_ self) -> Result, Error> { - return self.reader.try_dequeue(); + self.reader.try_dequeue() } pub fn try_send_data(&self, data: &[u8]) -> Result<(), Error> { - return self.writer.enqueue_copy(data); + self.writer.enqueue_copy(data) } pub fn send_or_discard_data(&self, data: &[u8]) { @@ -252,8 +252,8 @@ impl< fn bootloader_checksum_u32(word: u32) -> u8 { let word_b: [u8; 4] = word.to_be_bytes(); - let cks = word_b[0] ^ word_b[1] ^ word_b[2] ^ word_b[3]; - cks + + word_b[0] ^ word_b[1] ^ word_b[2] ^ word_b[3] } fn bootloader_checksum_buf(buf: &[u8]) -> u8 { @@ -379,7 +379,7 @@ impl< self.reader .read(|buf| { // defmt::info!("go cmd reply {:?}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); } else { @@ -389,9 +389,7 @@ impl< }) .await?; - if res.is_err() { - return res; - } + res?; // defmt::debug!("sending the load address {:X}...", write_base_addr); self.writer @@ -412,7 +410,7 @@ impl< self.reader .read(|buf| { defmt::trace!("load address reply {:X}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); // defmt::trace!("load address accepted."); @@ -441,7 +439,7 @@ impl< self.reader .read(|buf| { // defmt::trace!("send data reply {:X}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); // defmt::trace!("data accepted."); @@ -509,7 +507,7 @@ impl< self.reader .read(|buf| { defmt::trace!("extended erase reply {:?}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); } else { @@ -533,7 +531,7 @@ impl< self.reader .read(|buf| { defmt::debug!("erase reply {:?}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { defmt::debug!("flash erased"); res = Ok(()); @@ -556,14 +554,10 @@ impl< defmt::trace!("device is already in bootloader"); } else { defmt::trace!("resetting device into bootloader"); - if let Err(err) = self.reset_into_bootloader().await { - return Err(err); - } + self.reset_into_bootloader().await? } - if let Err(err) = self.verify_bootloader().await { - return Err(err); - } + self.verify_bootloader().await?; match self.get_device_id().await { Err(err) => return Err(err), @@ -585,14 +579,10 @@ impl< }; // erase part - if let Err(err) = self.erase_flash_memory().await { - return Err(err); - } + self.erase_flash_memory().await?; // program image - if let Err(err) = self.write_device_memory(fw_image_bytes, None).await { - return Err(err); - } + self.write_device_memory(fw_image_bytes, None).await?; self.reset_into_program(leave_in_reset).await; @@ -627,7 +617,7 @@ impl< self.reader .read(|buf| { defmt::info!("go cmd reply {:?}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); } else { @@ -637,9 +627,7 @@ impl< }) .await?; - if res.is_err() { - return res; - } + res?; defmt::debug!("sending the start address {:?}...", start_address); self.writer @@ -660,7 +648,7 @@ impl< self.reader .read(|buf| { defmt::info!("sa reply {:?}", buf); - if buf.len() >= 1 { + if !buf.is_empty() { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); self.in_bootloader = false; diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index cda0793c..52065ddb 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -274,10 +274,7 @@ impl< .dequeue(|buf| { // defmt::warn!("buf {}", buf); let packet = self.to_packet(buf); - if packet.is_err() { - defmt::warn!("parsed invalid packet"); - } else { - let packet = packet.unwrap(); + if let Ok(packet) = packet { if let EdmPacket::ATEvent(ATEvent::NetworkDown { interface_id: 0, }) = packet @@ -295,6 +292,8 @@ impl< } else { defmt::warn!("got edm packet: {}", packet); } + } else { + defmt::warn!("parsed invalid packet"); } }) .await; @@ -528,7 +527,7 @@ impl< } Err(queue::Error::QueueFull) => { // nothing to read - return Err(OdinRadioError::ReadLowLevelBufferEmpty); + Err(OdinRadioError::ReadLowLevelBufferEmpty) } Err(queue::Error::QueueEmpty) => { // nothing to read diff --git a/lib-stm32/src/power/battery.rs b/lib-stm32/src/power/battery.rs index 7fda85ab..6c56199d 100644 --- a/lib-stm32/src/power/battery.rs +++ b/lib-stm32/src/power/battery.rs @@ -72,7 +72,7 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { if self.cell_voltage_compute_mode == CellVoltageComputeMode::Chained { for i in (1..NUM_CELLS).rev() { - self.cell_voltages[i] = self.cell_voltages[i] - self.cell_voltages[i - 1]; + self.cell_voltages[i] -= self.cell_voltages[i - 1]; } } @@ -123,7 +123,7 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { pub fn battery_warn(&self) -> bool { self.battery_cell_imbalance_warn() - || self.get_cell_voltages().into_iter().any(|cell_voltage| { + || self.get_cell_voltages().iter().any(|cell_voltage| { *cell_voltage > self.config.cell_voltage_high_warn || *cell_voltage < self.config.cell_voltage_low_warn }) @@ -135,7 +135,7 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { pub fn battery_crit(&self) -> bool { self.get_worst_cell_imbalance() > self.config.cell_voltage_difference_crit - || self.get_cell_voltages().into_iter().any(|cell_voltage| { + || self.get_cell_voltages().iter().any(|cell_voltage| { *cell_voltage > self.config.cell_voltage_high_crit || *cell_voltage < self.config.cell_voltage_low_crit }) @@ -143,7 +143,7 @@ impl<'a, const NUM_CELLS: usize, F: Filter> LipoModel<'a, NUM_CELLS, F> { pub fn battery_power_off(&self) -> bool { self.get_worst_cell_imbalance() > self.config.cell_votlage_difference_off - || self.get_cell_voltages().into_iter().any(|cell_voltage| { + || self.get_cell_voltages().iter().any(|cell_voltage| { *cell_voltage > self.config.cell_voltage_high_power_off || *cell_voltage < self.config.cell_voltage_low_power_off }) diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index 9f28de0b..09621902 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -265,7 +265,7 @@ impl Queue { if write_index == 0 { write_index = DEPTH - 1; } else { - write_index = write_index - 1; + write_index -= 1; } // write back decremented write index. Write index is incremented to the next buffer diff --git a/lib-stm32/src/time/mod.rs b/lib-stm32/src/time/mod.rs index d2de1fb7..9a5a4498 100644 --- a/lib-stm32/src/time/mod.rs +++ b/lib-stm32/src/time/mod.rs @@ -8,7 +8,7 @@ pub struct SyncTicker { impl SyncTicker { pub fn every(duration: Duration) -> Self { Self { - duration: duration, + duration, ready_at: Instant::now() + duration, } } @@ -17,6 +17,7 @@ impl SyncTicker { self.ready_at = Instant::now() + self.duration; } + #[allow(clippy::should_implement_trait)] // this is mimicking the embassy ticker pub fn next(&mut self) -> bool { let cur_time = Instant::now(); if cur_time >= self.ready_at { @@ -32,6 +33,12 @@ pub struct Limiter { prev_val: Option, } +impl Default for Limiter { + fn default() -> Self { + Self::new() + } +} + impl Limiter { pub const fn new() -> Self { Limiter { prev_val: None } diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 6227a7bd..324b46ba 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -256,12 +256,12 @@ impl< pub async fn update_uart_config(&self, config: usart::Config) -> Result<(), ()> { #[cfg(target_has_atomic)] - if let Err(_) = self.uart_config_update_in_progress.compare_exchange( + if self.uart_config_update_in_progress.compare_exchange( false, true, Ordering::Acquire, Ordering::Relaxed, - ) { + ).is_err() { return Err(()); } @@ -377,12 +377,12 @@ impl< } #[cfg(target_has_atomic)] - if let Err(_) = self.uart_config_update_in_progress.compare_exchange( + if self.uart_config_update_in_progress.compare_exchange( true, false, Ordering::SeqCst, Ordering::Acquire, - ) { + ).is_err() { defmt::error!( "uart config update state became inchoerant. This should not be possible." ); From f3b57c978918f70e1045d7e120448ec6f853cab5 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 20:33:28 -0400 Subject: [PATCH 17/22] style --- control-board/src/drivers/kicker.rs | 3 ++- control-board/src/drivers/radio_robot.rs | 2 +- control-board/src/drivers/shell_indicator.rs | 5 ++++- control-board/src/lib.rs | 5 +---- control-board/src/stspin_motor.rs | 3 ++- control-board/src/tasks/audio_task.rs | 3 ++- control-board/src/tasks/imu_task.rs | 22 +++++++++++++++++-- control-board/src/tasks/power_task.rs | 5 ++++- control-board/src/tasks/radio_task.rs | 3 ++- kicker-board/src/bin/hwtest-blinky/main.rs | 3 ++- kicker-board/src/bin/hwtest-charge/main.rs | 3 ++- kicker-board/src/bin/hwtest-coms/main.rs | 3 ++- kicker-board/src/bin/hwtest-kick/main.rs | 3 ++- kicker-board/src/bin/hwtest-kickstr/main.rs | 3 ++- kicker-board/src/drivers/breakbeam.rs | 8 +++---- kicker-board/src/drivers/mod.rs | 3 ++- .../bin/hwtest-uart-queue-mixedbaud/main.rs | 3 ++- .../bin/hwtest-uart-queue-mixedrate/main.rs | 3 ++- lib-stm32/src/drivers/boot/stm32_interface.rs | 4 ++-- lib-stm32/src/drivers/flash/at25df041b.rs | 3 ++- lib-stm32/src/drivers/imu/bmi323.rs | 3 ++- lib-stm32/src/drivers/led/apa102.rs | 3 ++- lib-stm32/src/drivers/other/adc_helper.rs | 5 ++++- lib-stm32/src/drivers/switches/button.rs | 3 ++- lib-stm32/src/drivers/switches/dip.rs | 5 ++++- .../src/drivers/switches/rotary_encoder.rs | 5 ++++- lib-stm32/src/lib.rs | 5 ++--- lib-stm32/src/time/mod.rs | 2 +- lib-stm32/src/uart/queue.rs | 22 +++++++++---------- power-board/src/tasks/audio_task.rs | 3 ++- power-board/src/tasks/coms_task.rs | 5 ++++- power-board/src/tasks/power_task.rs | 3 ++- 32 files changed, 100 insertions(+), 54 deletions(-) diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 984f804a..96387329 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -4,7 +4,8 @@ use ateam_lib_stm32::{ }; use embassy_stm32::{ gpio::{AnyPin, Pull}, - usart::Parity, Peri, + usart::Parity, + Peri, }; use embassy_time::{with_timeout, Duration, Timer}; diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 2d0f7bdb..82ba3da6 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -13,8 +13,8 @@ use core::mem::size_of; use credentials::WifiCredential; use embassy_futures::select::{select, Either}; use embassy_stm32::gpio::{AnyPin, Level, Output, Speed}; -use embassy_stm32::{uid, Peri}; use embassy_stm32::usart::{self, DataBits, Parity, StopBits}; +use embassy_stm32::{uid, Peri}; use embassy_time::{Duration, Timer}; use heapless::String; diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index de277c97..3a06c6ec 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -1,4 +1,7 @@ -use embassy_stm32::{gpio::{AnyPin, Level, Output, Speed}, Peri}; +use embassy_stm32::{ + gpio::{AnyPin, Level, Output, Speed}, + Peri, +}; pub struct ShellIndicator<'a> { fr_pin0: Output<'a>, diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index b1299d32..6cdc956d 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -4,10 +4,7 @@ #![feature(inherent_associated_types)] #![feature(generic_const_exprs)] #![feature(type_alias_impl_trait)] -#![feature( - maybe_uninit_slice, - maybe_uninit_write_slice -)] +#![feature(maybe_uninit_slice, maybe_uninit_write_slice)] #![feature(ptr_metadata)] #![feature(sync_unsafe_cell)] #![feature(variant_count)] diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 5409a392..e7608f07 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -7,7 +7,8 @@ use ateam_lib_stm32::{ use defmt::*; use embassy_stm32::{ gpio::{AnyPin, Pull}, - usart::Parity, Peri, + usart::Parity, + Peri, }; use embassy_time::{with_timeout, Duration, Timer}; use nalgebra::Vector3; diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index 11ef7fc8..6434237c 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -6,7 +6,8 @@ use embassy_stm32::{ timer::{ simple_pwm::{PwmPin, SimplePwm}, Channel, - }, Peri, + }, + Peri, }; use embassy_time::{Duration, Ticker}; diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 55a4fbb7..4756e7df 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -243,7 +243,16 @@ pub fn start_imu_task( // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; - let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss.into(), imu_buf); + let imu = Bmi323::new_from_pins( + peri, + sck, + mosi, + miso, + txdma, + rxdma, + bmi323_nss.into(), + imu_buf, + ); // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain @@ -290,7 +299,16 @@ pub fn start_imu_task_via_ie( // let imu_buf: &'static mut [u8; 14] = unsafe { & mut IMU_BUFFER_CELL }; let imu_buf: &mut [u8; bmi323::SPI_MIN_BUF_LEN] = unsafe { &mut (*(&raw mut IMU_BUFFER_CELL)) }; - let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss.into(), imu_buf); + let imu = Bmi323::new_from_pins( + peri, + sck, + mosi, + miso, + txdma, + rxdma, + bmi323_nss.into(), + imu_buf, + ); // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain diff --git a/control-board/src/tasks/power_task.rs b/control-board/src/tasks/power_task.rs index 08dac759..ce075f32 100644 --- a/control-board/src/tasks/power_task.rs +++ b/control-board/src/tasks/power_task.rs @@ -6,7 +6,10 @@ use ateam_lib_stm32::{ uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, }; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::{usart::{self, DataBits, Parity, StopBits, Uart}, Peri}; +use embassy_stm32::{ + usart::{self, DataBits, Parity, StopBits, Uart}, + Peri, +}; use embassy_time::{Duration, Instant, Ticker, Timer}; use crate::{ diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 3366817a..c7bc1774 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -8,7 +8,8 @@ use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ gpio::{AnyPin, Input, Pull}, - usart::{self, DataBits, Parity, StopBits, Uart}, Peri, + usart::{self, DataBits, Parity, StopBits, Uart}, + Peri, }; use embassy_time::{Duration, Instant, Ticker, Timer}; diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 1bdca500..3075747b 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -11,7 +11,8 @@ use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, spi::{Config, Spi}, - time::mhz, Peri, + time::mhz, + Peri, }; use embassy_time::{Duration, Timer}; diff --git a/kicker-board/src/bin/hwtest-charge/main.rs b/kicker-board/src/bin/hwtest-charge/main.rs index 3ea870d6..e4e891ea 100644 --- a/kicker-board/src/bin/hwtest-charge/main.rs +++ b/kicker-board/src/bin/hwtest-charge/main.rs @@ -11,7 +11,8 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, - gpio::{Level, Output, Speed}, Peri, + gpio::{Level, Output, Speed}, + Peri, }; use embassy_time::{Duration, Ticker, Timer}; diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 3f5c6b58..18e3e121 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -14,7 +14,8 @@ use embassy_stm32::{ gpio::{Level, Output, Speed}, interrupt::{self, InterruptExt}, pac::Interrupt, - usart::{Config, Parity, StopBits, Uart}, Peri, + usart::{Config, Parity, StopBits, Uart}, + Peri, }; use embassy_stm32::{bind_interrupts, peripherals, usart}; diff --git a/kicker-board/src/bin/hwtest-kick/main.rs b/kicker-board/src/bin/hwtest-kick/main.rs index 28cabb96..72d89266 100644 --- a/kicker-board/src/bin/hwtest-kick/main.rs +++ b/kicker-board/src/bin/hwtest-kick/main.rs @@ -12,7 +12,8 @@ use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, - opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, Peri, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + Peri, }; use embassy_time::{Duration, Ticker, Timer}; diff --git a/kicker-board/src/bin/hwtest-kickstr/main.rs b/kicker-board/src/bin/hwtest-kickstr/main.rs index b7826133..8e56c026 100644 --- a/kicker-board/src/bin/hwtest-kickstr/main.rs +++ b/kicker-board/src/bin/hwtest-kickstr/main.rs @@ -20,7 +20,8 @@ use embassy_executor::Executor; use embassy_stm32::{ adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, - opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, Peri, + opamp::{OpAmp, OpAmpGain, OpAmpSpeed}, + Peri, }; use embassy_time::{Duration, Ticker, Timer}; diff --git a/kicker-board/src/drivers/breakbeam.rs b/kicker-board/src/drivers/breakbeam.rs index 1473c678..845a28e8 100644 --- a/kicker-board/src/drivers/breakbeam.rs +++ b/kicker-board/src/drivers/breakbeam.rs @@ -1,5 +1,6 @@ use embassy_stm32::{ - gpio::{AnyPin, Input, Level, Output, Pull, Speed}, Peri, + gpio::{AnyPin, Input, Level, Output, Pull, Speed}, + Peri, }; pub struct Breakbeam<'a> { @@ -8,10 +9,7 @@ pub struct Breakbeam<'a> { } impl<'a> Breakbeam<'a> { - pub fn new( - pin_tx: Peri<'a, AnyPin>, - pin_rx: Peri<'a, AnyPin>, - ) -> Self { + pub fn new(pin_tx: Peri<'a, AnyPin>, pin_rx: Peri<'a, AnyPin>) -> Self { let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); let pin_rx = Input::new(pin_rx, Pull::Down); Self { pin_tx, pin_rx } diff --git a/kicker-board/src/drivers/mod.rs b/kicker-board/src/drivers/mod.rs index 7875acd6..bb0c2d9a 100644 --- a/kicker-board/src/drivers/mod.rs +++ b/kicker-board/src/drivers/mod.rs @@ -14,7 +14,8 @@ use ateam_lib_stm32::{ }; use embassy_stm32::{ gpio::{AnyPin, Pull}, - usart::Parity, Peri, + usart::Parity, + Peri, }; use embassy_time::{with_timeout, Duration, Timer}; diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index b6b586a8..139b71b3 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -11,7 +11,8 @@ use embassy_stm32::{ gpio::{Level, Output, Pull, Speed}, interrupt, peripherals::{self, *}, - usart::{self, *}, Peri, + usart::{self, *}, + Peri, }; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index a62117be..0f4615f0 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -12,7 +12,8 @@ use embassy_stm32::{ interrupt, pac::Interrupt, peripherals::{self, *}, - usart::{self, *}, Peri, + usart::{self, *}, + Peri, }; use embassy_time::Timer; diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index b3509d6d..ca697fe3 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -252,7 +252,7 @@ impl< fn bootloader_checksum_u32(word: u32) -> u8 { let word_b: [u8; 4] = word.to_be_bytes(); - + word_b[0] ^ word_b[1] ^ word_b[2] ^ word_b[3] } @@ -591,7 +591,7 @@ impl< 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."); - + if !self.in_bootloader { defmt::error!("called bootloader operation when not in bootloader context."); return Err(()); diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index 74b19959..9dc70054 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -1,7 +1,8 @@ use embassy_stm32::{ gpio::{AnyPin, Level, Output, Speed}, mode::Async, - spi::{self, Error}, Peri, + spi::{self, Error}, + Peri, }; pub struct AT25DF041B<'buf, const CS_POL_N: bool> { diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 7a9d70f2..1624560a 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -10,7 +10,8 @@ use embassy_stm32::{ gpio::{AnyPin, Level, Output, Speed}, mode::Async, spi::{self, MisoPin, MosiPin, SckPin}, - time::hz, Peri, + time::hz, + Peri, }; pub const SPI_MIN_BUF_LEN: usize = 14; diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index 124f8eeb..7bf51028 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -3,7 +3,8 @@ use core::ops::Range; use embassy_stm32::{ mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, - time::mhz, Peri, + time::mhz, + Peri, }; use smart_leds::RGB8; diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 5729fc3e..9ca4eba9 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -1,4 +1,7 @@ -use embassy_stm32::{adc::{self, Adc, AdcChannel, Resolution, SampleTime}, Peri}; +use embassy_stm32::{ + adc::{self, Adc, AdcChannel, Resolution, SampleTime}, + Peri, +}; // The voltage which the internal ADC were calibrated at. // For the H743 and F407 diff --git a/lib-stm32/src/drivers/switches/button.rs b/lib-stm32/src/drivers/switches/button.rs index f7d2495f..ba67b35b 100644 --- a/lib-stm32/src/drivers/switches/button.rs +++ b/lib-stm32/src/drivers/switches/button.rs @@ -2,7 +2,8 @@ use defmt::Format; use embassy_futures::select; use embassy_stm32::{ exti::ExtiInput, - gpio::{Pin, Pull}, Peri, + gpio::{Pin, Pull}, + Peri, }; use embassy_time::{Instant, Timer}; diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs index 2e1ab1f1..2af6d5c1 100644 --- a/lib-stm32/src/drivers/switches/dip.rs +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -3,7 +3,10 @@ use core::{ ops::Range, }; -use embassy_stm32::{gpio::{AnyPin, Input, Pull}, Peri}; +use embassy_stm32::{ + gpio::{AnyPin, Input, Pull}, + Peri, +}; pub struct DipSwitch<'a, const PIN_CT: usize> { inputs: [Input<'a>; PIN_CT], diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs index 56ed79b0..8259ccea 100644 --- a/lib-stm32/src/drivers/switches/rotary_encoder.rs +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -1,4 +1,7 @@ -use embassy_stm32::{gpio::{AnyPin, Input, Pull}, Peri}; +use embassy_stm32::{ + gpio::{AnyPin, Input, Pull}, + Peri, +}; pub struct RotaryEncoder<'a, const PIN_CT: usize> { pins: [Input<'a>; PIN_CT], diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 23775ea6..aa984f2c 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -1,11 +1,10 @@ #![no_std] - #![allow(incomplete_features)] -#![allow(clippy::too_many_arguments)] // too many functions passing pins to device drivers exceed the bound +#![allow(clippy::too_many_arguments)] +// too many functions passing pins to device drivers exceed the bound // if "strict" feature is on, promote warnings to errors #![cfg_attr(feature = "strict", deny(warnings))] - #![feature(generic_const_exprs)] #![feature(const_precise_live_drops)] #![feature(sync_unsafe_cell)] diff --git a/lib-stm32/src/time/mod.rs b/lib-stm32/src/time/mod.rs index 9a5a4498..6130b1d4 100644 --- a/lib-stm32/src/time/mod.rs +++ b/lib-stm32/src/time/mod.rs @@ -17,7 +17,7 @@ impl SyncTicker { self.ready_at = Instant::now() + self.duration; } - #[allow(clippy::should_implement_trait)] // this is mimicking the embassy ticker + #[allow(clippy::should_implement_trait)] // this is mimicking the embassy ticker pub fn next(&mut self) -> bool { let cur_time = Instant::now(); if cur_time >= self.ready_at { diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 324b46ba..c2753629 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -256,12 +256,11 @@ impl< pub async fn update_uart_config(&self, config: usart::Config) -> Result<(), ()> { #[cfg(target_has_atomic)] - if self.uart_config_update_in_progress.compare_exchange( - false, - true, - Ordering::Acquire, - Ordering::Relaxed, - ).is_err() { + if self + .uart_config_update_in_progress + .compare_exchange(false, true, Ordering::Acquire, Ordering::Relaxed) + .is_err() + { return Err(()); } @@ -377,12 +376,11 @@ impl< } #[cfg(target_has_atomic)] - if self.uart_config_update_in_progress.compare_exchange( - true, - false, - Ordering::SeqCst, - Ordering::Acquire, - ).is_err() { + if self + .uart_config_update_in_progress + .compare_exchange(true, false, Ordering::SeqCst, Ordering::Acquire) + .is_err() + { defmt::error!( "uart config update state became inchoerant. This should not be possible." ); diff --git a/power-board/src/tasks/audio_task.rs b/power-board/src/tasks/audio_task.rs index 635c9c2f..2c8563f3 100644 --- a/power-board/src/tasks/audio_task.rs +++ b/power-board/src/tasks/audio_task.rs @@ -10,7 +10,8 @@ use embassy_stm32::{ timer::{ simple_pwm::{PwmPin, SimplePwm}, Channel, - }, Peri, + }, + Peri, }; use embassy_time::{Duration, Ticker, Timer}; diff --git a/power-board/src/tasks/coms_task.rs b/power-board/src/tasks/coms_task.rs index 2e42a3c9..0c72ac4b 100644 --- a/power-board/src/tasks/coms_task.rs +++ b/power-board/src/tasks/coms_task.rs @@ -1,6 +1,9 @@ use core::mem::MaybeUninit; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::{usart::{self, DataBits, Parity, StopBits, Uart}, Peri}; +use embassy_stm32::{ + usart::{self, DataBits, Parity, StopBits, Uart}, + Peri, +}; use crate::{pins::*, power_state::SharedPowerState, SystemIrqs, DEBUG_UART_QUEUES}; use ateam_common_packets::bindings::{BatteryInfo, PowerCommand, PowerTelemetry}; diff --git a/power-board/src/tasks/power_task.rs b/power-board/src/tasks/power_task.rs index 775e9262..1ee3bcc9 100644 --- a/power-board/src/tasks/power_task.rs +++ b/power-board/src/tasks/power_task.rs @@ -13,7 +13,8 @@ use ateam_lib_stm32::{ use embassy_executor::Spawner; use embassy_stm32::{ adc::{Adc, AdcChannel, AnyAdcChannel, SampleTime}, - peripherals::ADC1, Peri, + peripherals::ADC1, + Peri, }; use embassy_time::{Duration, Instant, Ticker, Timer}; From a671e8388f1373e327800f8bd70c4ec24a76abdf Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 25 Aug 2025 20:40:18 -0400 Subject: [PATCH 18/22] remove all targets from clippy lint --- .github/workflows/rustlint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rustlint.yml b/.github/workflows/rustlint.yml index 7a431c94..635a90ec 100644 --- a/.github/workflows/rustlint.yml +++ b/.github/workflows/rustlint.yml @@ -36,4 +36,4 @@ jobs: - name: Common Libraries Clippy Default shell: bash run: | - nix develop -c cargo clippy --all-targets --all-features --manifest-path lib-stm32/Cargo.toml + nix develop -c cargo clippy --all-features --manifest-path lib-stm32/Cargo.toml From 9716a64e13fe99c63de5dc5ee4fb1a0a97c2aa8f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 26 Aug 2025 19:20:56 -0400 Subject: [PATCH 19/22] fix startup panic --- control-board/.cargo/config.toml | 2 +- control-board/src/tasks/radio_task.rs | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index 19e930b0..dd405941 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -3,7 +3,7 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] # runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' -runner = 'probe-rs run --chip STM32H723ZGTx' +runner = 'probe-rs run --preverify --chip STM32H723ZGTx' [env] diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index c7bc1774..9335be5f 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -341,6 +341,8 @@ impl< )) .await; } + + radio_loop_rate_ticker.reset(); } RadioConnectionState::ConnectedNetwork => { if let Ok(connected) = self @@ -354,7 +356,6 @@ impl< // Refresh last software packet on first connect. self.last_software_packet = Instant::now(); self.connection_state = RadioConnectionState::Connected; - radio_loop_rate_ticker.reset(); self.led_command_pub .publish(ControlBoardLedCommand::Radio( RadioStatusLedCommand::ConnectedSoftware, @@ -380,6 +381,8 @@ impl< )) .await; } + + radio_loop_rate_ticker.reset(); } RadioConnectionState::Connected => { let _ = self.process_packets(tx_ctr).await; @@ -607,8 +610,6 @@ impl< // always send the latest telemetry if tx_ctr == 0 { - defmt::info!("RadioTask - sending basic telemetry"); - self.last_basic_telemetry.transmission_sequence_number = self.seq_number as u8; self.seq_number = (self.seq_number + 1) & 0x00FF; From 37fececf18307fa8cffd14616c94792ee7f685fd Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 26 Aug 2025 19:40:31 -0400 Subject: [PATCH 20/22] remove old cargo toml comments --- kicker-board/Cargo.toml | 2 +- lib-stm32/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 9e87cf88..80279758 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -31,7 +31,7 @@ embassy-stm32 = { version = "0.3.0", features = [ embassy-futures = { version = "0.1.0" } embassy-sync = { version = "0.7.1" } -defmt = "1.0.1" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt = "1.0.1" defmt-rtt = "1.0.0" panic-probe = { version = "1.0.0", features = ["print-defmt"] } diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 320b5eff..846092b2 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -14,7 +14,7 @@ embassy-futures = { version = "0.1.0" } embassy-time = { version = "0.4.0" } paste = { version = "1.0.15" } -defmt = "1.0.1" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt = "1.0.1" defmt-rtt = "1.0.0" critical-section = "1.1.1" From a6cab42fe6e9b932e7d87f4960c4ccb383246ebd Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Sep 2025 15:04:36 -0400 Subject: [PATCH 21/22] update dependencies to embassy executor 9.1 base --- control-board/Cargo.lock | 74 +++++++++++++++++++++++++-------------- control-board/Cargo.toml | 10 +++--- kicker-board/Cargo.lock | 74 +++++++++++++++++++++++++-------------- kicker-board/Cargo.toml | 10 +++--- lib-stm32-test/Cargo.lock | 74 +++++++++++++++++++++++++-------------- lib-stm32-test/Cargo.toml | 10 +++--- lib-stm32/Cargo.lock | 63 ++++++++++++++++++++------------- lib-stm32/Cargo.toml | 10 +++--- power-board/Cargo.lock | 74 +++++++++++++++++++++++++-------------- power-board/Cargo.toml | 10 +++--- 10 files changed, 252 insertions(+), 157 deletions(-) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 447ab01d..c1cdc67d 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -172,9 +172,9 @@ checksum = "d21f40d350a700f6aa107e45fb26448cf489d34794b2ba4522181dc9f1173af6" [[package]] name = "bit_field" -version = "0.10.2" +version = "0.10.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" +checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitfield" @@ -472,8 +472,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -490,20 +490,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", ] [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -511,15 +514,26 @@ dependencies = [ "syn", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" + [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -530,15 +544,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -586,8 +600,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -600,8 +614,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -616,25 +630,25 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -642,8 +656,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1281,8 +1295,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1587,3 +1602,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index cc34d27e..dfc94213 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -12,18 +12,18 @@ cortex-m-rt = "0.7.5" defmt = "1.0.1" defmt-rtt = "1.0.0" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", ] } -embassy-time = { version = "0.4.0", features = [ +embassy-time = { version = "0.5.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32h723zg", "unstable-pac", @@ -31,8 +31,8 @@ embassy-stm32 = { version = "0.3.0", features = [ "exti", "chrono" ] } -embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.7.1" } +embassy-futures = { version = "0.1.2" } +embassy-sync = { version = "0.7.2" } static_cell = "2.1.1" futures-util = { version = "0.3.31", default-features = false } diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index a0c0a08d..6dd7459f 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -142,9 +142,9 @@ dependencies = [ [[package]] name = "bit_field" -version = "0.10.2" +version = "0.10.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" +checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitfield" @@ -438,8 +438,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -456,20 +456,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", ] [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -477,15 +480,26 @@ dependencies = [ "syn", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" + [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -496,15 +510,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -552,8 +566,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -566,8 +580,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -582,25 +596,25 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -608,8 +622,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1247,8 +1261,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1547,3 +1562,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 80279758..4b36af45 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -8,18 +8,18 @@ edition = "2021" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", ] } -embassy-time = { version = "0.4.0", features = [ +embassy-time = { version = "0.5.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32g474ve", "dual-bank", # CHECK, this is probably critical @@ -28,8 +28,8 @@ embassy-stm32 = { version = "0.3.0", features = [ "exti", "chrono" ] } -embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.7.1" } +embassy-futures = { version = "0.1.2" } +embassy-sync = { version = "0.7.2" } defmt = "1.0.1" defmt-rtt = "1.0.0" diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index a65f971a..ff9e9402 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -79,9 +79,9 @@ dependencies = [ [[package]] name = "bit_field" -version = "0.10.2" +version = "0.10.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" +checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitfield" @@ -325,8 +325,8 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -343,20 +343,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", ] [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -364,15 +367,26 @@ dependencies = [ "syn", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" + [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -383,15 +397,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -439,8 +453,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -453,8 +467,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -469,25 +483,25 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -495,8 +509,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -864,8 +878,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -941,3 +956,8 @@ checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" dependencies = [ "vcell", ] + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index 4b525767..a1542b47 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -10,18 +10,18 @@ cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" defmt = "1.0.1" defmt-rtt = "1.0.0" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", ] } -embassy-time = { version = "0.4.0", features = [ +embassy-time = { version = "0.5.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32h723zg", "unstable-pac", @@ -29,9 +29,9 @@ embassy-stm32 = { version = "0.3.0", features = [ "exti", "chrono" ] } -embassy-futures = { version = "0.1.0" } +embassy-futures = { version = "0.1.2" } futures-util = { version = "0.3.31", default-features = false } -embassy-sync = { version = "0.7.1" } +embassy-sync = { version = "0.7.2" } panic-probe = { version = "1.0.0", features = ["print-defmt"] } static_cell = "2.1.1" critical-section = "1.2.0" diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 3a80eeb6..bd2b4cd5 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -57,9 +57,9 @@ dependencies = [ [[package]] name = "bit_field" -version = "0.10.2" +version = "0.10.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" +checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitfield" @@ -264,8 +264,8 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embassy-futures", "embassy-hal-internal", @@ -280,18 +280,21 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "critical-section", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue", ] [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -299,15 +302,21 @@ dependencies = [ "syn", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -317,12 +326,12 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -365,8 +374,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -378,8 +387,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -393,8 +402,8 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] @@ -402,15 +411,15 @@ dependencies = [ [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "embassy-sync", @@ -752,8 +761,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", ] @@ -821,3 +831,8 @@ checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" dependencies = [ "vcell", ] + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 846092b2..b6089fd0 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -7,11 +7,11 @@ description = "A common library for ateam stm32 targets" repository = "https://github.com/SSL-A-Team/firmware" [dependencies] -embassy-stm32 = { version = "0.3.0", default-features = false, features = ["exti"]} -embassy-executor = { version = "0.8.0", default-features = false } -embassy-sync = { version = "0.7.1" } -embassy-futures = { version = "0.1.0" } -embassy-time = { version = "0.4.0" } +embassy-stm32 = { version = "0.4.0", default-features = false, features = ["exti"]} +embassy-executor = { version = "0.9.1", default-features = false } +embassy-sync = { version = "0.7.2" } +embassy-futures = { version = "0.1.2" } +embassy-time = { version = "0.5.0" } paste = { version = "1.0.15" } defmt = "1.0.1" diff --git a/power-board/Cargo.lock b/power-board/Cargo.lock index 208254d3..003404e8 100644 --- a/power-board/Cargo.lock +++ b/power-board/Cargo.lock @@ -129,9 +129,9 @@ dependencies = [ [[package]] name = "bit_field" -version = "0.10.2" +version = "0.10.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" +checksum = "1e4b40c7323adcfc0a41c4b88143ed58346ff65a288fc144329c5c45e05d70c6" [[package]] name = "bitfield" @@ -416,8 +416,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -434,20 +434,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", ] [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -455,15 +458,26 @@ dependencies = [ "syn", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" + [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -474,15 +488,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -529,8 +543,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -543,8 +557,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -559,25 +573,25 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -585,8 +599,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1136,8 +1150,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1430,3 +1445,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/power-board/Cargo.toml b/power-board/Cargo.toml index 88ce7f8c..68e76dc1 100644 --- a/power-board/Cargo.toml +++ b/power-board/Cargo.toml @@ -9,26 +9,26 @@ repository = "https://github.com/SSL-A-Team/firmware" cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", ] } -embassy-time = { version = "0.4.0", features = [ +embassy-time = { version = "0.5.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32g030c8", "unstable-pac", "time-driver-tim1", "exti", ] } -embassy-futures = { version = "0.1.0" } -embassy-sync = { version = "0.7.1" } +embassy-futures = { version = "0.1.2" } +embassy-sync = { version = "0.7.2" } defmt = "1.0.1" defmt-rtt = "1.0.0" From 8e30837d8dfee43d3ed0a58652689d29b481cf4b Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Sep 2025 15:13:59 -0400 Subject: [PATCH 22/22] push embassy deps forward --- control-board/Cargo.lock | 71 ++++++++++++++++++--------------------- control-board/Cargo.toml | 4 +-- kicker-board/Cargo.lock | 56 +++++++++++++++++------------- kicker-board/Cargo.toml | 4 +-- lib-stm32-test/Cargo.lock | 51 +++++++++++++++------------- lib-stm32-test/Cargo.toml | 4 +-- lib-stm32/Cargo.lock | 47 ++++++++++++++------------ lib-stm32/Cargo.toml | 6 ++-- power-board/Cargo.lock | 58 ++++++++++++++++++-------------- power-board/Cargo.toml | 4 +-- 10 files changed, 160 insertions(+), 145 deletions(-) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index ae200ea0..c1cdc67d 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -170,21 +170,6 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d21f40d350a700f6aa107e45fb26448cf489d34794b2ba4522181dc9f1173af6" -[[package]] -name = "bit_field" -version = "0.10.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5020822f6d6f23196ccaf55e228db36f9de1cf788052b37992e17cbc96ec41a7" -dependencies = [ - "bisync_macros", -] - -[[package]] -name = "bisync_macros" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d21f40d350a700f6aa107e45fb26448cf489d34794b2ba4522181dc9f1173af6" - [[package]] name = "bit_field" version = "0.10.3" @@ -487,8 +472,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -505,8 +490,9 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", @@ -519,7 +505,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -540,13 +527,13 @@ source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894 [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -557,15 +544,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -613,8 +600,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -627,8 +614,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -643,16 +630,16 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", @@ -661,7 +648,7 @@ dependencies = [ [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -669,8 +656,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1308,8 +1295,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1614,3 +1602,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index dff075fa..5221b9f6 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -12,7 +12,7 @@ cortex-m-rt = "0.7.5" defmt = "1.0.1" defmt-rtt = "1.0.0" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", @@ -23,7 +23,7 @@ embassy-time = { version = "0.5.0", features = [ "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32h723zg", "unstable-pac", diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 3dee81b4..6dd7459f 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -438,8 +438,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -456,8 +456,9 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", @@ -470,7 +471,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -491,13 +493,13 @@ source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894 [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -508,15 +510,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -564,8 +566,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -578,8 +580,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -594,16 +596,16 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", @@ -612,7 +614,7 @@ dependencies = [ [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -620,8 +622,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1259,8 +1261,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1559,3 +1562,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 822507c4..ee365a1f 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -8,7 +8,7 @@ edition = "2021" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", @@ -19,7 +19,7 @@ embassy-time = { version = "0.5.0", features = [ "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32g474ve", "dual-bank", # CHECK, this is probably critical diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index 94468bb8..ff9e9402 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -325,8 +325,8 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -343,8 +343,9 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", @@ -357,7 +358,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -378,13 +380,13 @@ source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894 [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -395,15 +397,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -451,8 +453,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -465,8 +467,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -481,16 +483,16 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", @@ -499,7 +501,7 @@ dependencies = [ [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -507,8 +509,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -876,8 +878,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index 4b4e0a1b..b661228f 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -10,7 +10,7 @@ cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" defmt = "1.0.1" defmt-rtt = "1.0.0" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", @@ -21,7 +21,7 @@ embassy-time = { version = "0.5.0", features = [ "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32h723zg", "unstable-pac", diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index fa144cc0..bd2b4cd5 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -264,8 +264,8 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embassy-futures", "embassy-hal-internal", @@ -280,8 +280,9 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "critical-section", "document-features", @@ -292,7 +293,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -308,13 +310,13 @@ checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -324,12 +326,12 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -372,8 +374,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -385,8 +387,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -400,8 +402,8 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] @@ -409,15 +411,15 @@ dependencies = [ [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "embedded-io-async", ] [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "embassy-sync", @@ -759,8 +761,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", ] diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 846092b2..2a6841a9 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -7,11 +7,11 @@ description = "A common library for ateam stm32 targets" repository = "https://github.com/SSL-A-Team/firmware" [dependencies] -embassy-stm32 = { version = "0.3.0", default-features = false, features = ["exti"]} -embassy-executor = { version = "0.8.0", default-features = false } +embassy-stm32 = { version = "0.4.0", default-features = false, features = ["exti"]} +embassy-executor = { version = "0.9.1", default-features = false } embassy-sync = { version = "0.7.1" } embassy-futures = { version = "0.1.0" } -embassy-time = { version = "0.4.0" } +embassy-time = { version = "0.5.0" } paste = { version = "1.0.15" } defmt = "1.0.1" diff --git a/power-board/Cargo.lock b/power-board/Cargo.lock index 6f1295bf..003404e8 100644 --- a/power-board/Cargo.lock +++ b/power-board/Cargo.lock @@ -416,8 +416,8 @@ checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "embassy-embedded-hal" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embassy-futures", @@ -434,8 +434,9 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.8.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", @@ -448,7 +449,8 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.7.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -469,13 +471,13 @@ source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894 [[package]] name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.1.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" [[package]] name = "embassy-hal-internal" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cortex-m", "critical-section", @@ -486,15 +488,15 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", ] [[package]] name = "embassy-stm32" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.4.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "aligned", "bit_field", @@ -541,8 +543,8 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.1" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.7.2" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -555,8 +557,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.4.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "cfg-if", "critical-section", @@ -571,25 +573,25 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.2.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "document-features", ] [[package]] name = "embassy-time-queue-utils" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue 0.1.0 (git+https://github.com/embassy-rs/embassy)", "heapless 0.8.0", ] [[package]] name = "embassy-usb-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -597,8 +599,8 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#ef673c6ca310cf0a7e9b1254afb7806bd6879e94" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" dependencies = [ "critical-section", "defmt 1.0.1", @@ -1148,8 +1150,9 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "17.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-ecb93d42a6cbcd9e09cab74873908a2ca22327f7#6f80b256aa7e41d34394ccf6fa127475d455d4c3" +version = "18.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1442,3 +1445,8 @@ name = "windows_x86_64_msvc" version = "0.53.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486" + +[[patch.unused]] +name = "embassy-executor" +version = "0.9.0" +source = "git+https://github.com/embassy-rs/embassy#de33d113a5fb70ca5086058cc894a2ce192c27d6" diff --git a/power-board/Cargo.toml b/power-board/Cargo.toml index 4dccaefa..62732050 100644 --- a/power-board/Cargo.toml +++ b/power-board/Cargo.toml @@ -9,7 +9,7 @@ repository = "https://github.com/SSL-A-Team/firmware" cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.5" -embassy-executor = { version = "0.8.0", features = [ +embassy-executor = { version = "0.9.1", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", @@ -20,7 +20,7 @@ embassy-time = { version = "0.5.0", features = [ "defmt-timestamp-uptime", "tick-hz-32_768", ] } -embassy-stm32 = { version = "0.3.0", features = [ +embassy-stm32 = { version = "0.4.0", features = [ "defmt", "stm32g030c8", "unstable-pac",