diff --git a/.gitignore b/.gitignore index 06e1beb..c208279 100644 --- a/.gitignore +++ b/.gitignore @@ -45,3 +45,4 @@ fp-info-cache # Rust generated files controlSystem/RecoveryBoard/firmware-rs/target controlSystem/RecoveryBoard/firmware-rs/Cargo.lock +controlSystem/RecoveryBoard/firmware-rs/__pycache__ diff --git a/controlSystem/RecoveryBoard/firmware-rs/.cargo/config.toml b/controlSystem/RecoveryBoard/firmware-rs/.cargo/config.toml index 68cdf13..72b8374 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/.cargo/config.toml +++ b/controlSystem/RecoveryBoard/firmware-rs/.cargo/config.toml @@ -1,6 +1,10 @@ [target.thumbv6m-none-eabi] runner = 'probe-rs run --chip STM32F091RCTX' +[alias] +sender = "run -r --bin sender" +parachute = "run -r --bin parachute" + [build] target = "thumbv6m-none-eabi" diff --git a/controlSystem/RecoveryBoard/firmware-rs/Cargo.toml b/controlSystem/RecoveryBoard/firmware-rs/Cargo.toml index 1c560f7..3729de2 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/Cargo.toml +++ b/controlSystem/RecoveryBoard/firmware-rs/Cargo.toml @@ -21,6 +21,17 @@ embedded-hal-nb = "1.0.0" noline = "0.5.1" heapless = "0.9.1" format_no_std = "1.2.0" +embedded-test-linker-script = { version = "0.1.0" } + +[dev-dependencies] +embedded-test = { version = "0.7.0", features=["embassy", "defmt"] } + +[lib] +harness = false # Important: As we bring our own test harness for all tests + +[[test]] +name = "blinky" +harness = false [profile.release] debug = 2 diff --git a/controlSystem/RecoveryBoard/firmware-rs/README.md b/controlSystem/RecoveryBoard/firmware-rs/README.md index 425d024..d52765e 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/README.md +++ b/controlSystem/RecoveryBoard/firmware-rs/README.md @@ -23,6 +23,8 @@ With all the dependencies installed, you should now be able to navigate to the d cargo run --bin blinky --release ``` +If you are flashing blinky directly onto one of the ERS boards, you will need to set the BOARD environment variable to `"main"`. + After it builds, terminal output should look like this if everything went according to plan: ```bash @@ -38,3 +40,25 @@ Programming ✔ 100% [####################] 15.00 KiB @ 20.53 KiB/s (took 1s) ``` and the led should be blinking. + +## Flashing + +The commands to flash the binaries are as follows: + +*Sender*: + +```rust +cargo sender +``` + +*Drogue parachute*: + +```rust +BOARD=drogue cargo parachute +``` + +*Main parachute*: + +```rust +BOARD=main cargo parachute +``` diff --git a/controlSystem/RecoveryBoard/firmware-rs/build.rs b/controlSystem/RecoveryBoard/firmware-rs/build.rs index 8cd32d7..632c25b 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/build.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/build.rs @@ -1,5 +1,21 @@ +use std::env; + fn main() { + println!("cargo::rerun-if-env-changed=BOARD"); println!("cargo:rustc-link-arg-bins=--nmagic"); println!("cargo:rustc-link-arg-bins=-Tlink.x"); println!("cargo:rustc-link-arg-bins=-Tdefmt.x"); + println!("cargo::rustc-link-arg=-Tembedded-test.x"); + println!("cargo:rustc-link-arg-tests=-Tdefmt.x"); + println!("cargo:rustc-link-arg-tests=-Tlink.x"); + + println!("cargo::rustc-check-cfg=cfg(drogue,main)"); + if let Ok(board) = env::var("BOARD") { + if board == "drogue" { + println!("cargo::rustc-cfg=drogue"); + } + if board == "main" { + println!("cargo::rustc-cfg=main"); + } + } } diff --git a/controlSystem/RecoveryBoard/firmware-rs/echo_can.py b/controlSystem/RecoveryBoard/firmware-rs/echo_can.py new file mode 100644 index 0000000..bd446bc --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/echo_can.py @@ -0,0 +1,52 @@ +""" +This script is used to test that your can bus is set up properly. +With a VulCAN plugged in, run this script in one terminal, +making sure to pass in the proper path to the VulCAN with the `-d` flag. +Then have the ERS board or Nucleo run the `echo_can` task in another. +You should see the two boards echoing CAN messages back and forth. +* Requires python-can v4.5, pySerial v3.5 * +""" + +import can +import argparse +import logging +from typing import cast + +logger = logging.getLogger(__name__) +logging.basicConfig( + format="%(asctime)s - [%(levelname)s]: %(message)s", + datefmt="%H:%M:%S", + level=logging.INFO, +) + +parser = argparse.ArgumentParser( + prog="VulCAN Tester", + description="Send or Receive messages via CAN using a VulCAN adapter", +) + +_ = parser.add_argument("--device", "-d", required=True, help="Path to VulCAN") + +args = parser.parse_args() + +# Ensure proper arg types +device = cast(str, args.device) +device = str(device) + + +def main(): + logger.info("Connecting to VulCAN") + bitrate = 1e6 + + # Configure the connection to the VulCAN + bus = can.interface.Bus(channel="can0", interface="socketcan", bitrate=bitrate) + + logger.info("Echoing CAN messages") + while True: + message = bus.recv(timeout=None) + if message: + bus.send(message) + logger.info("Echoed message: %s", message) + + +if __name__ == "__main__": + main() diff --git a/controlSystem/RecoveryBoard/firmware-rs/spoof_ers_can.py b/controlSystem/RecoveryBoard/firmware-rs/spoof_ers_can.py index 22704f9..7f7b52b 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/spoof_ers_can.py +++ b/controlSystem/RecoveryBoard/firmware-rs/spoof_ers_can.py @@ -1,4 +1,11 @@ -# Requires python-can v4.5, pySerial v3.5 +""" +This script uses the Vulcan to spoof the other two parachute boards when you only have one board to use. +It's purpose is to test that the sender board is functioning properly. +Make sure to pass in the path to your Vulcan with the `-d` flag when invoking this script. +You should see that the Vulcan is sending good status messages over the CAN bus that match both drogue and main parachute board heartbeat IDs. +* Requires python-can v4.5, pySerial v3.5 * +""" + import can import time import argparse @@ -33,17 +40,26 @@ def main(): # Configure the connection to the VulCAN bus = can.interface.Bus(channel="can0", interface="socketcan", bitrate=bitrate) + status_buf = [ + 2, # Ring locked + 100, # Battery voltage in 0.1 volts (> 9.9v is good) + 1, # Batt_ok == True + 1, # Shore power Off == true + 1, # Received sender message in last two seconds + 1, # Board is ready to release parachute + 0, # unused + 0, # unused + ] + while True: message_id = 0x710 - data_bytes = [0x01] msg = can.Message( - arbitration_id=message_id, data=data_bytes, is_extended_id=False + arbitration_id=message_id, data=status_buf, is_extended_id=False ) bus.send(msg) message2_id = 0x720 - data_bytes2 = [0x01] msg2 = can.Message( - arbitration_id=message2_id, data=data_bytes2, is_extended_id=False + arbitration_id=message2_id, data=status_buf, is_extended_id=False ) bus.send(msg2) logger.info("Sent messages: %s %s", msg, msg2) diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/README.md b/controlSystem/RecoveryBoard/firmware-rs/src/README.md new file mode 100644 index 0000000..0925278 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/README.md @@ -0,0 +1,363 @@ +# Shared Modules + +This README explains all modules in the src directory. The modules are used by both the `parachute.rs` and `sender.rs` binaries. +- [adc.rs](#Adc) +- [blink.rs](#Blink) +- [buzzer.rs](#Buzzer) +- [can.rs](#Can) +- [motor.rs](#Motor) +- [ring.rs](#Ring) +- [types.rs](#Types) +- [uart.rs](#Uart) + +## Adc + +The adc module defines shared analog-to-digital converter functionalities between boards. It creates a [Watch](https://docs.rs/embassy-sync/latest/embassy_sync/watch/struct.Watch.html) signal that holds the most recent battery reading, and the mutex that is used by the parachute board to ensure safe access to the adc driver. + +This module defines two tasks, `read_battery` and `read_battery_from_ref`. The former is used by the sender board, as it can pass the adc driver directly, and the latter is used by the parachute boards, because they need to use the mutex. This is because the adc is used to both read the battery and read the ring position in the parachute boards. + +## Blink + +This module defines the task that blinks the LED on all 3 boards to indicate operational status. + +## Buzzer + +The buzzer module manages the audible feedback that all 3 boards use to indicate their status. It uses an enum to determine which mode it should be in: High, Low, or Off. It uses PWM to control the buzzer, and thus depends on a PWM driver to be passed in by the binaries. When the task begins, it plays a short melody. Then it enters a loop that unlocks the mutex that contains its mode enum. It matches the mode enum and will play a quicker, higher-pitched beep if its in High mode, or a less frequent, lower pitched beep if its in Low mode. Otherwise, it does nothing. + +## Can + +The can module defines several constants, creates a [channel](#https://docs.rs/embassy-sync/latest/embassy_sync/channel/struct.Channel.html), defines the `CanTxChannelMsg` struct, and defines two tasks, `echo_can` and `can_writer`. + +### Constants + +- **`CAN_BITRATE: u32 = 1_000_000`** + The CAN bus communication speed set to 1 Mbps (1,000,000 bits per second). This high-speed configuration ensures rapid message delivery between all ERS system components. + +- **`DROGUE_DEPLOY_ID: u16 = 0x100`** + Command message ID for deploying the drogue parachute. Sent by the sender board to initiate drogue release. + +- **`DROGUE_ACKNOWLEDGE_ID: u16 = 0x101`** + Acknowledgment message ID sent by the drogue parachute board to confirm it received and is executing the deployment command. + +- **`DROGUE_HEARTBEAT_ID: u16 = 0x710`** + Status message ID used by the drogue parachute board to broadcast its current state (ready, motor status, sensor readings, etc.). + +- **`MAIN_DEPLOY_ID: u16 = 0x200`** + Command message ID for deploying the main parachute. Sent by the sender board to initiate main parachute release. + +- **`MAIN_ACKNOWLEDGE_ID: u16 = 0x201`** + Acknowledgment message ID sent by the main parachute board to confirm it received and is executing the deployment command. + +- **`MAIN_HEARTBEAT_ID: u16 = 0x720`** + Status message ID used by the main parachute board to broadcast its current state (ready, motor status, sensor readings, etc.). + +- **`SENDER_HEARTBEAT_ID: u16 = 0x700`** + Periodic status message ID used by the sender board to broadcast overall system health, including: + - Rocket readiness status + - Battery voltage and health + - Shore power status + - Parachute board communication status + - Deployment signal status + +The message IDs follow a numbering scheme: +- **0x1XX**: Drogue parachute commands and acknowledgments +- **0x2XX**: Main parachute commands and acknowledgments +- **0x7XX**: System status and heartbeat messages + +### Tasks + +- `echo_can` + Debug task that will simply write a message to the bus, and then enter a loop that waits for a response which it then echoes back to the bus again. To be used with `echo_can.py`. + +- `can_writer` + Handles writing messages to the CAN bus for all boards. The channel used by this task only accepts a CanTxChannelMsg, which is a simple wrapper around Embassy's CAN Frame, that tells the writer task if it should write the message with a blocking write, or if it should only try to write the message and fail with an error if it can't. + +## Motor + +The motor driver module provides control and management of the parachute deployment motor system. It handles motor direction, current limiting, and position-based control with safety features. + +### Constants + +- **`MOTOR_DRIVE_CURR_MA`**: Maximum current limit for motor operation (2000 mA) +- **`MOTOR_DRIVE_DUR_MS`**: Default timeout duration for motor operations + +### Struct Fields + +- `deploy1`: Output pin for motor direction control (forward) +- `deploy2`: Output pin for motor direction control (reverse) +- `ps`: Power save control pin +- `motor_fail`: Input pin for motor failure detection +- `dac`: DAC peripheral for current limiting control + +### `MotorMode` + +Enum defining the possible motor operating modes: + +- `PowerSave`: Minimum power consumption mode +- `Stop`: Motor stopped but powered +- `Forward`: Drive motor forward (toward unlocked position) +- `Reverse`: Drive motor reverse (toward locked position) +- `Brake`: Active braking mode + +### Methods + +#### `Motor::new()` + +Creates a new motor controller instance. + +**Parameters:** +- `pb4`: GPIO pin for deploy1 control +- `pb5`: GPIO pin for deploy2 control +- `pb6`: GPIO pin for power save control +- `pb7`: GPIO pin for motor failure input +- `dac`: DAC peripheral for current limiting + +#### `set_mode()` + +Sets the motor operating mode by controlling the output pins. + +**Parameters:** +- `mode`: `MotorMode` enum value specifying desired operation + +#### `limit_motor_current()` + +Configures the DAC to limit motor current to the specified value. + +**Parameters:** +- `ma`: Desired current limit in milliamps (0-2000 mA) + +**Implementation Details:** +- Uses circuit analysis to convert mA to DAC value +- Formula: `scale = (ma * 1024) / 1375` +- Sets 12-bit right-aligned DAC value for current control + +#### `read_ring_pos_until_condition()` + +Monitors ring position and motor current until target position is reached. + +**Parameters:** +- `position`: Target `RingPosition` to wait for + +**Behavior:** +- Continuously reads ring position and motor current sensors +- Writes out CSV of `MOTOR_ISENSE` pin values over defmt rtt. +- Returns when target position is detected + +#### `drive()` + +Main motor driving function with comprehensive control and safety features. + +**Parameters:** +- `mode`: Target `RingPosition` (Locked or Unlocked) +- `duration_ms`: Maximum drive time in milliseconds +- `force`: Whether to ignore sensor feedback +- `current`: Motor current limit in milliamps + +**Operation:** +1. Sets current limit via DAC +2. Stops motor initially +3. Drives in specified direction based on target position +4. Monitors either position feedback or uses timeout +5. Returns to power save mode when complete + +## Ring + +The ring position sensor module provides real-time monitoring and position detection for the parachute deployment ring system. It uses hall effect sensors to determine the ring's position (locked, unlocked, or intermediate states) and monitors motor current consumption. + +### Statics + +- **`RingType`**: Thread-safe mutex wrapper for ring controller: `Mutex>` + +- **`RING_MTX`**: Global mutex for ring controller access + +- **`RING_POSITION_WATCH`**: Broadcasts current `RingPosition` state +- **`SENSOR_READ_WATCH`**: Broadcasts raw sensor readings from both hall sensors +- **`MOTOR_ISENSE_WATCH`**: Broadcasts motor current sense readings + +### Enums + +#### `RingPosition` + +Defines the possible states of the parachute deployment ring: + +- **`Locked`**: Ring is in the locked position (parachute secured) +- **`Unlocked`**: Ring is in the unlocked position (parachute deployed) +- **`Inbetween`**: Ring is between locked and unlocked positions +- **`Error`**: Invalid or unexpected sensor readings detected + +#### `SensorState` + +Internal state machine for individual sensor interpretation: + +- **`Active`**: Sensor reading indicates active/magnet-present state +- **`Unactive`**: Sensor reading indicates inactive/no-magnet state +- **`Under`**: Reading below minimum valid threshold +- **`Over`**: Reading above maximum valid threshold +- **`Inbetween`**: Reading in transition zone between states + +### Structures + +#### `SensorReadings` + +Container for raw ADC readings from both hall sensors: + +**Fields:** +- `sensor1`: Raw ADC value from first hall sensor (PA0) +- `sensor2`: Raw ADC value from second hall sensor (PA1) + +**Methods:** +- `new(sensor1, sensor2)`: Creates new sensor readings instance + +#### `SensorLimits` + +Calibration thresholds for interpreting sensor readings: + +**Fields:** +- `over`: Upper threshold for "over-range" detection +- `under`: Lower threshold for "under-range" detection +- `active`: Threshold for active sensor state +- `unactive`: Threshold for inactive sensor state + +**Methods:** +- `new(over, under, active, unactive)`: Creates calibrated limit set + +#### `Ring Struct` + +Main ring position controller managing sensor hardware and position detection: + +**Fields:** +- `pa0`: First hall effect sensor input +- `pa1`: Second hall effect sensor input +- `pb1`: Motor current sense input +- `sensor1_limits`: Calibration limits for sensor 1 +- `sensor2_limits`: Calibration limits for sensor 2 + +#### Methods + +##### `Ring::new()` + +Creates a new ring position controller with calibrated sensor limits. + +**Parameters:** +- `pa0`: First hall sensor peripheral +- `pa1`: Second hall sensor peripheral +- `pb1`: Motor current sense peripheral + +**Calibration Values:** +- **Sensor 1**: Over=3700, Under=600, Active=2100, Unactive=900 +- **Sensor 2**: Over=3700, Under=600, Active=1300, Unactive=900 + +**NOTE**: Calibration are experimentally derived and may need to be changed. + +##### `broadcast_ring_position()` + +Main position detection routine that reads sensors, interprets position, and broadcasts updates. + +**Operation Flow:** +1. Acquires ADC mutex for sensor reading access +2. Reads all three analog inputs: + - Sensor 1 (PA0) - Hall effect position + - Sensor 2 (PA1) - Hall effect position + - Motor current sense (PB1) +3. Broadcasts raw readings via watch signals +4. Interprets sensor states using calibrated thresholds +5. Determines ring position based on sensor state combination +6. Broadcasts final ring position + +### Internal Helper Functions + +#### `get_sensor_state()` + +Converts raw ADC values to discrete sensor states using calibrated limits. + +**Parameters:** +- `adc_val`: Raw 12-bit ADC reading (0-4095) +- `limit`: `SensorLimits` structure with calibration thresholds + +**State Determination Logic:** +- **Over**: `adc_val >= limit.over` +- **Under**: `adc_val <= limit.under` +- **Active**: `adc_val >= limit.active && adc_val < limit.over` +- **Unactive**: `adc_val <= limit.unactive && adc_val > limit.under` +- **Inbetween**: Values between active/unactive thresholds + +#### `get_ring_position()` + +Determines overall ring position based on combined sensor states. + +**Position Logic:** +- **Unlocked**: + - Sensor1 Active AND Sensor2 not Active + - Sensor1 not Unactive AND Sensor2 Unactive +- **Locked**: + - Sensor1 not Active AND Sensor2 Active + - Sensor1 Unactive AND Sensor2 not Unactive +- **Inbetween**: Either sensor in Inbetween state +- **Error**: All other combinations + +### Tasks + +#### `read_pos_sensor` + +Background task that continuously monitors ring position at 50ms intervals. + +**Operation:** +1. Acquires ring controller mutex +2. Calls `broadcast_ring_position()` to update all watch signals +3. Waits 50ms before next reading +4. Runs indefinitely in loop + +## Types + +This module contains various type definitions that may be used in more than one place. + +## Uart + +Provides buffered UART communication with async I/O traits implementation for CLI interface. Used by CLI task for command-line interface with noline crate compatibility. + +### Constants + +- **`UART_BUF_SIZE: usize = 1024`** + Size of transmit and receive buffers (1KB each) + +### Static Resources + +See [static cell](https://docs.rs/static_cell/latest/static_cell/) + +#### Buffer Allocation + +``` +UART_TX_BUF_CELL // Transmit buffer static cell +UART_RX_BUF_CELL // Receive buffer static cell +``` + +**Purpose:** Statically allocated buffers with compile-time known sizes for zero-allocation UART operation. + +### Structures + +#### `IO` + +UART I/O wrapper that implements embedded-io-async traits. + +**Fields:** +- `stdio`: Buffered UART driver instance + +**Methods:** +- `new(stdio)`: Creates new IO wrapper + +#### `Error` + +Simple error type for I/O operations. + +#### Traits Implementation + +##### `embedded_io_async::Read` + +- **`read()`**: Reads data from UART into buffer +- Returns bytes read or I/O error + +##### `embedded_io_async::Write` + +- **`write()`**: Writes buffer data to UART +- **`flush()`**: Ensures all data transmitted +- Returns bytes written or I/O error diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/adc.rs b/controlSystem/RecoveryBoard/firmware-rs/src/adc.rs similarity index 62% rename from controlSystem/RecoveryBoard/firmware-rs/src/shared/adc.rs rename to controlSystem/RecoveryBoard/firmware-rs/src/adc.rs index 2397a09..c6ff42d 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/shared/adc.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/adc.rs @@ -1,13 +1,15 @@ -use crate::shared::types::*; +use crate::types::*; use embassy_stm32::{ adc::Adc, peripherals::{ADC1, PB0}, Peri, }; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex, watch::Watch}; use embassy_time::Timer; -pub static BATT_READ_CHANNEL: Channel = Channel::new(); +pub static BATT_READ_WATCH: Watch = Watch::new(); + +pub static ADC_MTX: AdcType = Mutex::new(None); #[embassy_executor::task] pub async fn read_battery(mut adc: Adc<'static, ADC1>, mut pb0: Peri<'static, PB0>) { @@ -15,23 +17,26 @@ pub async fn read_battery(mut adc: Adc<'static, ADC1>, mut pb0: Peri<'static, PB let adc_read = adc.read(&mut pb0).await; let v_batt = ((adc_read as f32) / 4096.0 * 3.3) / 0.2326; let batt_sig = (v_batt / 0.1) as u8; - BATT_READ_CHANNEL.send(batt_sig).await; + BATT_READ_WATCH.sender().send(batt_sig); Timer::after_secs(1).await; } } #[embassy_executor::task] -pub async fn read_battery_from_ref(adc: &'static AdcType, mut pb0: Peri<'static, PB0>) { +pub async fn read_battery_from_ref(mut pb0: Peri<'static, PB0>) { loop { + let mut batt_sig: u8 = 0; + { - let mut adc_unlocked = adc.lock().await; + let mut adc_unlocked = ADC_MTX.lock().await; if let Some(adc_ref) = adc_unlocked.as_mut() { let adc_read = adc_ref.read(&mut pb0).await; let v_batt = ((adc_read as f32) / 4096.0 * 3.3) / 0.2326; - let batt_sig = (v_batt / 0.1) as u8; - BATT_READ_CHANNEL.send(batt_sig).await; + batt_sig = (v_batt / 0.1) as u8; } } + + BATT_READ_WATCH.sender().send(batt_sig); Timer::after_secs(1).await; } } diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/bin/README.md b/controlSystem/RecoveryBoard/firmware-rs/src/bin/README.md new file mode 100644 index 0000000..c1a839c --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/bin/README.md @@ -0,0 +1,255 @@ +# Board Binaries directory + +The src/bin directory contains the actual binaries that get flashed to the boards. + + - [blinky.rs](#blinky) + - [parachute.rs](#parachute) + - [sender.rs](#sender) + +## Blinky + +This binary is a simple testing program that blinks the LED and prints `"Hello, World"` over RTT. +It's purpose is to verify that your development environment is set up properly. +It can be flashed to either a NUCLEOf091rc development board or one of the actual PSAS ERS boards. +The command to flash it is `cargo run -r --bin blinky` +To flash it to a NUCLEO leave the BOARD environment variable unset. +To flash it to an ERS board, set the BOARD environment variable to `"main"` + +## Parachute + +This binary is the program for either the drogue or the main ERS parachute boards. +To flash it as the drogue board, run `BOARD=drogue cargo parachute`. +To flash it as the main board, run `BOARD=main cargo parachute`. + +### Interrupts + +the f091rc MCU bundles all of the CAN and CEC interrupts together into one interrupt. Because of this, we must bind the Rx0, Rx1, Tx, and Sce interrupt handlers to this one interrupt. +The Embassy devs have confirmed that the handlers are written in such a way that calling them unnecessarily does nothing, so this isn't an issue. +We also bind the adc interrupt to the imported adc interrupt handler, and the Usart interrupt gets bound to the BufferedInterruptHandler, as we use a buffered usart driver in this program. + +### State + +The state fields for the parachute boards are as follows: + +- `id`: U8 that's 1 if it's flashed as the drogue board, or 2 if it's flashed as the main board. + +- `ready`: Bool that will be true if shorepower is off, the battery is ok, and the sender heartbeat is good. + +- `shore_power_status`: Bool that indicates whether shore power is on or off. + +- `sender_last_seen`: U64 timestamp of the last time the sender board heartbeat was received. + +To set the state, there is a function named `set_state()` that takes in a `ChuteStateField` enum variant. For example: + +```rust +set_state(ChuteStateField::Ready(true)).await; +``` + +The state is stored in a mutex so we must `.await` setting it. + +#### GPIO + +- PA8: Umbilical status input +- PA10: CAN shutdown control +- PA9: CAN silent mode control + +### Drivers + +#### PWM + +The PWM driver is imported from the `embassy_timer` module. It use pin B15 configured as a `PwmPin`, and is controlled by the TIM15 timer. There is one channel that controls the buzzer. The counting mode and initial frequency are irrelevant for this use case, but must be set anyway. + +#### CAN + +The CAN driver is imported from the `embassy_stm32` module. It used the CAN peripheral, pins A11 and A12, and the `CanIrqs` we created when binding the interrupt handlers to the `CEC_CAN` interrupt. The filter bank is set up to accept all CAN messages, but could be configured with a mask to only accept certain message IDs. However, there are currently so few messages on the bus that it really isnt necessary to use the filters at this point. The only relevant portion of the config is the bitrate. It is set to `CAN_BITRATE`, which is imported from the CAN module located in the src dir. + +#### ADC + +The ADC driver is imported from the `embassy_stm32` module. It uses the ADC1 peripheral and the `AdcIrqs` struct we created when binding the interrupt handlers to the `ADC1_COMP` interrupt. The sample time and resolution are currenntly set to the max possible values to maximize the accuracy of the adc readings. There is no reason to change these settings. + +#### UART + +The UART driver is imported from the `embassy_stm32` module, and configured with a baudrate of 115200, No parity, 8 data bits, and 1 Stop bit (8N1). It takes the `USART2` peripheral, pin A3 for RX, pin A2 for TX, TX and RX static cell buffers, the `UsartIrqs` struct we created when binding the `BufferedInterruptHandler` to the `USART2` interrupt. The tx/rx buffers must be created using static cells because they must have static lifetimes. More info [here](https://docs.rs/static_cell/latest/static_cell/). + +#### DAC + +The DAC driver is imported from the `embassy_stm32` module, and created using the DAC1 peripheral. DAC1 Ch1 uses DMA1_CH3 and the associated pin is A4. Ch1 could also use DMA2_CH3 instead if needed. DAC1 Ch2 uses DMA1_CH4 and the associated pin is A5. Ch2 could also use DMA2_CH4 instead if needed. + +#### MOTOR + +The motor driver is imported from the local `motor.rs` module. It uses pins B4 for deploy1, B5 for deploy2, B6 for MOTOR_PS, and B7 for MOTOR_FAILA. It also takes a static mutable reference to the dac which doesn't need to be put into a mutex because it is not shared with other tasks. + +#### Ring + +The ring driver is imported from the local `ring.rs` module. It's mainly responsible for reading the position of the ring and determining if it's locked or unlocked using two hall sensors. Therefore, it takes in pins A0, A1, and PB1. These are the two hall sensors, and the motor_isense, respectively. + +### Tasks + +Tasks in Embassy are similar to threads. They allow different functions to be ran essentially concurrently. See the [embassy docs](https://embassy.dev/book/) for more info. The parachute boards have a total of 6 tasks that the exector manages. + +#### `blink_led` + +Blinks the LED on pin B14 on and off regularly to indicate the board is functional. + +#### `active_beep` + +PWM's the buzzer on startup and regularly during operation to indicate the board is functional. + +#### `cli` + +Manages user input over UART via a cli-like interface. Several commands are available. + +- `help`: Display all available commands and a short explanation of each. + +- `state`: Print the internal state. + +- `l`: Move the ring towards the lock position. Adding the `--force` flag will ignore the sensor readings and drive the motor until timeout. Adding the `--pulse` flag will drive the motor for only 100ms rather than the full duration. + +- `u` Similar to l, this will move the ring towards the unlocked position instead. The `--force` and `--pulse` flags are the same as for `l`. + +- `pos`: Prints the current sensor readings and ring state. Adding `--poll` will repeatedly print the readings and ring state until the user interrupts it with any input. + +The cli task uses the [noline](https://docs.rs/noline/latest/noline/) crate to manage its interface. + +#### `read_battery_from_ref` + +This task is imported from the local `adc.rs` module. It unlocks the mutex containing the reference to the adc and sends the reading out over the `BATT_READ_WATCH` signaling primitive. More info on what a Watch is [here](https://docs.embassy.dev/embassy-sync/git/default/watch/struct.Watch.html). + +#### `read_pos_sensor` + +The ring struct has a method named `broadcast_ring_position` on it that will update the `RING_POSITION_WATCH` signal with the latest position of the ring: locked, unlocked, inbetween, or error. This task is imported from the local `ring.rs` module. It repeatedly unlocks the mutex that contains the reference to the ring struct and calls `broadcast_ring_position` every 50ms. + +#### `can_writer` + +The `can_writer` task is imported from the local `can.rs` module. It waits for a CAN message to come in over the `CAN_TX_CHANNEL` and will write the message to the CAN bus. A channel is similar to a watch, with a few key differences that make it more appropriate for this task. It's documentation can be found [here](https://docs.embassy.dev/embassy-sync/git/default/channel/struct.Channel.html). The channel only accepts a `CanTxChannelMsg`, which is a simple wrapper around Embassy's CAN Frame, that tells the writer task if it should write the message with a blocking write, or if it should only try to write the message and fail with an error if it can't. + +#### `can_reader` + +`can_reader` is the task that handles waiting for CAN messages to come in on the CAN bus and responding appropriately. It behaves differently depending on whether the board was flashed with the BOARD environment variable set to `"main"` or `"drogue"` + +- If the task receives a message with an ID matching `DROGUE_DEPLOY_ID`, and it was flashed with the drogue BOARD environment variable, it will unlock the motor mutex and drive the motor to unlock the ring. If it was flashed with the main BOARD environment variable, it will do nothing. + +- If the task receives a message with an ID matching `MAIN_DEPLOY_ID`, and it was flashed with the main BOARD environment variable, it will unlock the motor mutex and drive the motor to unlock the ring. Otherwise, it will do nothing. + +- If the task receives a message with an ID matching `SENDER_HEARTBEAT_ID`, it will update its internal `sender_last_seen` state field with the timestamp of the message. + +#### `parachute_heartbeat` + +This task sends a message over the CAN bus that communicates its state and whether its ready to drive the motor or not. Because it also reads many bits of state, it also updates the system state at the same time. This presents potential for improvement, as this isn't really what the heartbeat task should do. It should only be responsible for sending the CAN message. Unfortunately, due to time constraints this refactoring hasn't been a high priority. In a perfect world `main()` would be responsible for business logic such as updating important state fields, setting the buzzer's mode, printing messages when state changes, etc, and the heartbeat task would simply communicate this state to the CAN bus. + +It accomplishes sending this message mainly by constructing a status buffer that contains u8 representations of its internal state. It grabs receivers for the various Watch signals and unlocks global mutexes as needed and then conditionally sends messages based on which BOARD environment variable the board was flashed with. + +## Sender + +This binary is the program for the sender board, which acts as the central controller for the ERS system. It monitors parachute board status, handles deployment commands, and communicates rocket readiness. +To flash the board as the sender, run `cargo sender` + +### Interrupts + +The sender board uses the same interrupt bundling approach as the parachute boards: +- CAN and CEC interrupts are bundled into the `CEC_CAN` interrupt +- ADC uses the `ADC1_COMP` interrupt handler +- UART uses the `BufferedInterruptHandler` for the `USART2` interrupt + +### State + +The sender board maintains comprehensive system state: + +- `rocket_ready`: Bool indicating if all systems are go for launch +- `force_rocket_ready`: Bool that can override normal readiness checks +- `drogue_status`: Bool indicating drogue parachute board health +- `main_status`: Bool indicating main parachute board health +- `shore_power_status`: Bool indicating if shore power is connected +- `drogue_last_seen`: U64 timestamp of last drogue status message +- `main_last_seen`: U64 timestamp of last main status message +- `iso_drogue_last_seen`: U64 timestamp of last drogue deployment signal +- `iso_main_last_seen`: U64 timestamp of last main deployment signal + +State is managed through a `set_state()` function that takes `SenderStateField` enum variants: + +```rust +set_state(SenderStateField::RocketReady(true)).await; +``` + +### Drivers + +#### PWM + +The PWM driver controls the buzzer using TIM15 timer and pin PB15 configured as a `PwmPin`. It generates audible alerts for system status. + +#### CAN + +The CAN driver uses the CAN peripheral with pins PA11 and PA12, configured with a bitrate of `CAN_BITRATE`. The filter bank accepts all CAN messages since message filtering is handled in software. + +#### ADC + +The ADC driver uses ADC1 peripheral with maximum sample time and 12-bit resolution for accurate battery voltage monitoring on pin PB0. + +#### UART + +The UART driver uses USART2 peripheral with pins PA2 (TX) and PA3 (RX), configured for 115200 baud, 8N1 format. It uses buffered operation with static cells for TX/RX buffers. + +#### GPIO + +Multiple GPIO pins are used for critical functions: +- PA8: Umbilical status input +- PA6: Main parachute deployment signal input +- PA5: Drogue parachute deployment signal input +- PA7: Rocket Ready output signal +- PA10: CAN shutdown control +- PA9: CAN silent mode control + +### Tasks + +#### `blink_led` + +Blinks the LED on pin PB14 to indicate the board is operational. + +#### `active_beep` + +Controls the buzzer to provide audible status indications. Different beep patterns indicate system states like rocket readiness. + +#### `cli` + +Provides a command-line interface over UART for system monitoring and control: + +- `help`: Display all available commands +- `state`: Print internal system state +- `drogue`: Send drogue deployment command +- `main`: Send main deployment command +- `rr`: Toggle Rocket Ready signal override +- `batt`: Display current battery voltage +- `beep`: Toggle periodic beeping +- `version`: Display firmware version information +- `acts`: Print the count of motor actuations stored in flash memory +- `erase`: Erase motor actuation count data from flash memory + +#### `read_battery` + +Continuously monitors battery voltage via ADC and updates the `BATT_READ_WATCH` signal. + +#### `handle_iso_rising_edge` + +Listens for deployment signals from the telemetrum on two separate tasks (one for drogue, one for main). When a rising edge is detected, it verifies umbilical status and initiates the corresponding parachute deployment. + +#### `can_writer` + +Handles transmission of CAN messages from the `CAN_TX_CHANNEL`. Uses the same `CanTxChannelMsg` structure as parachute boards to support both blocking and non-blocking writes. + +#### `can_reader` + +Processes incoming CAN messages: +- Updates parachute board status and timestamps from status messages +- Records deployment acknowledgments from parachute boards +- Tracks communication health with parachute boards + +#### `telemetrum_heartbeat` + +The core system monitoring task that: +- Evaluates overall system readiness based on parachute board status, battery level, and shore power +- Controls the Rocket Ready output signal +- Sends periodic status messages over CAN +- Updates buzzer mode based on system state +- Monitors for state changes and logs them + +This task integrates multiple system aspects to determine if the rocket is ready for launch, considering factors like battery voltage, parachute board communication health, and manual overrides. Similarly to the parachute boards, a potential source of improvement would be refactoring the heartbeat to communicate the state rather than handling business logic that should be handled by `main()`. diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/bin/blinky.rs b/controlSystem/RecoveryBoard/firmware-rs/src/bin/blinky.rs index 2572be1..6ba1760 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/bin/blinky.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/bin/blinky.rs @@ -12,9 +12,12 @@ use {defmt_rtt as _, panic_probe as _}; async fn main(_spawner: Spawner) { let p = embassy_stm32::init(Default::default()); info!("Hello World!"); - //PA5 is the onboard LED on the Nucleo F091RC + let mut led = Output::new(p.PA5, Level::High, Speed::Low); + #[cfg(main)] + let mut led = Output::new(p.PB14, Level::High, Speed::Low); + loop { info!("high"); led.set_high(); diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/bin/drogue.rs b/controlSystem/RecoveryBoard/firmware-rs/src/bin/drogue.rs deleted file mode 100644 index 14af6e8..0000000 --- a/controlSystem/RecoveryBoard/firmware-rs/src/bin/drogue.rs +++ /dev/null @@ -1,457 +0,0 @@ -#![no_std] -#![no_main] - -use defmt::*; -use embassy_executor::Spawner; -use embassy_stm32::{ - adc::{Adc, InterruptHandler, SampleTime}, - bind_interrupts, - can::{ - Can, CanRx, Fifo, Id, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, - TxInterruptHandler, - }, - dac::Value, - gpio::{Input, Level, Output, OutputType, Pull, Speed}, - peripherals::{ADC1, CAN, PA0, PA1, USART2}, - time::Hertz, - timer::{ - low_level::CountingMode, - simple_pwm::{PwmPin, SimplePwm}, - }, - usart::{BufferedUart, Config as UartConfig, DataBits, Parity, StopBits}, - Peri, -}; -use embassy_stm32::{can::filter::Mask32, dac::Dac, usart::BufferedInterruptHandler}; -use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, channel::Channel, mutex::Mutex}; -use embassy_time::{Instant, Timer}; -use embedded_io_async::Write; -use firmware_rs::shared::{ - adc::read_battery_from_ref, - buzzer::{BuzzerMode, BuzzerModeMtxType}, - can::{can_writer, CAN_BITRATE, CAN_TX_CHANNEL, DROGUE_ID, MAIN_ID}, - types::*, - uart::{IO, UART_BUF_SIZE, UART_RX_BUF_CELL, UART_TX_BUF_CELL}, -}; -use noline::builder::EditorBuilder; -use {defmt_rtt as _, panic_probe as _}; - -#[derive(Default)] -pub struct DrogueState { - pub rocket_ready: bool, - pub force_rocket_ready: bool, - pub shore_power_status: bool, - pub gear_pos: u8, - pub sensor_state: u8, -} - -#[derive(Debug)] -pub enum DrogueStateField { - RocketReady(bool), - ForceRocketReady(bool), - ShorePowerStatus(bool), -} - -pub struct DrogueStateIter<'a> { - state_fields: &'a DrogueState, - index: usize, -} - -impl DrogueState { - pub fn iter(&self) -> DrogueStateIter<'_> { - DrogueStateIter { state_fields: self, index: 0 } - } -} - -impl<'a> Iterator for DrogueStateIter<'a> { - type Item = DrogueStateField; - - fn next(&mut self) -> Option { - let result = match self.index { - 0 => Some(DrogueStateField::RocketReady(self.state_fields.rocket_ready)), - 1 => Some(DrogueStateField::ForceRocketReady(self.state_fields.force_rocket_ready)), - 2 => Some(DrogueStateField::ShorePowerStatus(self.state_fields.shore_power_status)), - _ => None, - }; - - if result.is_some() { - self.index += 1; - } - - result - } -} - -impl core::fmt::Display for DrogueStateField { - fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { - match *self { - Self::RocketReady(val) => { - core::write!(f, "Rocket Ready: {}", if val { "YES" } else { "NO" }) - } - Self::ForceRocketReady(val) => { - core::write!(f, "Force Rocket Ready: {}", if val { "YES" } else { "NO" }) - } - Self::ShorePowerStatus(val) => { - core::write!(f, "Shore Power: {}", if val { "ON" } else { "OFF" }) - } - } - } -} - -async fn set_state(update: DrogueStateField) { - let mut unlocked = SYSTEM_STATE_MTX.lock().await; - if let Some(state) = unlocked.as_mut() { - match update { - DrogueStateField::RocketReady(val) => state.rocket_ready = val, - DrogueStateField::ForceRocketReady(val) => state.force_rocket_ready = val, - DrogueStateField::ShorePowerStatus(val) => state.shore_power_status = val, - } - } -} - -bind_interrupts!(struct CanIrqs { - CEC_CAN => - Rx0InterruptHandler, - Rx1InterruptHandler, - SceInterruptHandler, - TxInterruptHandler; -}); -bind_interrupts!(struct AdcIrqs { ADC1_COMP => InterruptHandler; }); -bind_interrupts!(struct UsartIrqs { USART2 => BufferedInterruptHandler; }); - -static UMB_ON_MTX: UmbOnType = Mutex::new(None); -static ADC_MTX: AdcType = Mutex::new(None); -static BUZZER_MODE_MTX: BuzzerModeMtxType = Mutex::new(None); -static SYSTEM_STATE_MTX: Mutex> = Mutex::new(None); -static DAC_MTX: DacType = Mutex::new(None); -static DEPLOY_1_MTX: Mutex>> = Mutex::new(None); -static DEPLOY_2_MTX: Mutex>> = Mutex::new(None); -static MOTOR_PS_MTX: Mutex>> = Mutex::new(None); - -static RING_POSITION_CHANNEL: Channel = Channel::new(); - -#[embassy_executor::main] -async fn main(spawner: Spawner) { - let p = embassy_stm32::init(Default::default()); - - let deploy_1 = Output::new(p.PB4, Level::Low, Speed::Medium); - let deploy_2 = Output::new(p.PB5, Level::Low, Speed::Medium); - let motor_ps = Output::new(p.PB6, Level::High, Speed::Medium); - let _motor_fail = Input::new(p.PB7, Pull::Up); - let umb_on = Input::new(p.PA8, Pull::Up); - let _can_shdn = Output::new(p.PA10, Level::Low, Speed::Medium); - let _can_silent = Output::new(p.PA9, Level::Low, Speed::Medium); - - // Set up PWM driver - let buzz_pin = PwmPin::new(p.PB15, OutputType::PushPull); - let _pwm = SimplePwm::new( - p.TIM15, - None, - Some(buzz_pin), - None, - None, - Hertz(440), - CountingMode::EdgeAlignedUp, - ); - - let buzzer_mode = BuzzerMode::Off; - - // Set up CAN driver - let mut can = Can::new(p.CAN, p.PA11, p.PA12, CanIrqs); - can.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all()); - can.modify_config().set_bitrate(CAN_BITRATE).set_loopback(false).set_silent(false); - - // Set up ADC driver - let mut adc = Adc::new(p.ADC1, AdcIrqs); - adc.set_sample_time(SampleTime::CYCLES239_5); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - - // Set up UART driver' - let mut uart_config = UartConfig::default(); - uart_config.baudrate = 115200; - uart_config.parity = Parity::ParityNone; - uart_config.data_bits = DataBits::DataBits8; - uart_config.stop_bits = StopBits::STOP1; - let uart = BufferedUart::new( - p.USART2, - p.PA3, - p.PA2, - UART_TX_BUF_CELL.take(), - UART_RX_BUF_CELL.take(), - UsartIrqs, - uart_config, - ) - .expect("Uart Config Error"); - - let dac = Dac::new(p.DAC1, p.DMA1_CH3, p.DMA1_CH4, p.PA4, p.PA5); - let sys_state = DrogueState::default(); - - { - // Put peripherals into mutex if shared among tasks. - // Inner scope so that mutex is unlocked when out of scope - *(BUZZER_MODE_MTX.lock().await) = Some(buzzer_mode); - *(ADC_MTX.lock().await) = Some(adc); - *(UMB_ON_MTX.lock().await) = Some(umb_on); - *(SYSTEM_STATE_MTX.lock().await) = Some(sys_state); - *(DAC_MTX.lock().await) = Some(dac); - *(DEPLOY_1_MTX.lock().await) = Some(deploy_1); - *(DEPLOY_2_MTX.lock().await) = Some(deploy_2); - *(MOTOR_PS_MTX.lock().await) = Some(motor_ps); - } - - // unwrap!(spawner.spawn(active_beep(pwm, None))); - unwrap!(spawner.spawn(cli(uart))); - unwrap!(spawner.spawn(read_battery_from_ref(&ADC_MTX, p.PB0))); - unwrap!(spawner.spawn(read_hall_sensor(p.PA0, p.PA1))); - - // enable at last minute so other tasks can still spawn if can bus is down - can.enable().await; - let (can_tx, can_rx) = can.split(); - unwrap!(spawner.spawn(can_writer(can_tx, &CAN_TX_CHANNEL))); - unwrap!(spawner.spawn(can_reader(can_rx, can,))); - - // Keep main from returning. Needed for can_tx/can_rx or they get dropped - core::future::pending::<()>().await; -} - -#[embassy_executor::task] -pub async fn cli(uart: BufferedUart<'static>) { - let prompt = "> "; - let mut io = IO::new(uart); - let mut buffer = [0; UART_BUF_SIZE]; - let mut history = [0; UART_BUF_SIZE]; - loop { - let mut editor = EditorBuilder::from_slice(&mut buffer) - .with_slice_history(&mut history) - .build_async(&mut io) - .await - .unwrap(); - - while let Ok(line) = editor.readline(prompt, &mut io).await { - // WARN: Passing more than 5 args panics - let args: heapless::Vec<&str, 5> = line.split_whitespace().skip(1).collect(); - if args.len() > 1usize { - error!("Only one argument allowed"); - continue; - } - match line { - "help" => { - let lines = [ - "help: Display this message.\r\n\n", - "state: Print internal state.\r\n\n", - "l: Move the ring towards the lock position.\r\n", - " --force: Ignore sensor readings, continue until timeout.\r\n", - " --pulse: Do a 100ms step instead of a full swing.\r\n\n", - "u: Move the ring towards the unlocked position.\r\n", - " --force: Ignore sensor readings, continue until timeout.\r\n", - " --pulse: Do a 100ms step instead of a full swing.\r\n\n", - "pos: Print the current sensor readings and ring state.\r\n", - " --poll: Print the sensor value and ring state every second.\r\n\n", - ]; - for line in lines { - io.write(line.as_bytes()).await.unwrap(); - } - io.flush().await.unwrap(); - } - "state" => { - let mut buf = [0u8; 64]; - let mut state_unlocked = SYSTEM_STATE_MTX.lock().await; - if let Some(state) = state_unlocked.as_mut() { - let time_now = Instant::now().as_millis(); - let ts = format_no_std::show( - &mut buf, - format_args!("Current time: {}ms\r\n", time_now), - ) - .unwrap(); - io.write(ts.as_bytes()).await.unwrap(); - for field in state.iter() { - let s = format_no_std::show(&mut buf, format_args!("{}\r\n", field)) - .unwrap(); - io.write(s.as_bytes()).await.unwrap(); - } - } - } - "l" => { - drive_motor(false, 50, false, 1000, RingPosition::Locked).await; - } - "u" => { - drive_motor(true, 50, false, 1000, RingPosition::Unlocked).await; - } - "pos" => { - // TODO: implement - } - _ => { - io.write(b"Invalid command\r\n").await.unwrap(); - } - } - } - } -} - -#[derive(Debug)] -enum RingPosition { - Locked, - Unlocked, - Inbetween, - Error, -} - -#[derive(Default)] -struct SensorLimits { - over: u16, - under: u16, - active: u16, - unactive: u16, -} - -impl SensorLimits { - fn new(over: u16, under: u16, active: u16, unactive: u16) -> Self { - Self { over, under, active, unactive } - } -} - -#[derive(PartialEq)] -enum SensorState { - Active, - Unactive, - Under, - Over, - Inbetween, -} - -fn get_sensor_state(adc_val: u16, limit: SensorLimits) -> SensorState { - if adc_val >= limit.over { - SensorState::Over - } else if adc_val <= limit.under { - SensorState::Under - } else if adc_val == limit.active { - SensorState::Active - } else if adc_val == limit.unactive { - SensorState::Unactive - } else { - SensorState::Inbetween - } -} - -fn get_ring_position(sensor1_state: SensorState, sensor2_state: SensorState) -> RingPosition { - if sensor1_state == SensorState::Active && sensor2_state != SensorState::Active { - return RingPosition::Locked; - } - if sensor1_state != SensorState::Unactive && sensor2_state == SensorState::Unactive { - return RingPosition::Locked; - } - if sensor1_state != SensorState::Active && sensor2_state == SensorState::Active { - return RingPosition::Unlocked; - } - if sensor1_state == SensorState::Unactive && sensor2_state != SensorState::Unactive { - return RingPosition::Unlocked; - } - if sensor1_state == SensorState::Inbetween || sensor2_state == SensorState::Inbetween { - RingPosition::Inbetween - } else { - RingPosition::Error - } -} - -async fn limit_motor_current(ma: u16) { - // TODO: Check if this is actually setting dac to the desired mA - let val = Value::Bit12Right(ma * 1024 / 1375); - let mut dac_unlocked = DAC_MTX.lock().await; - if let Some(dac) = dac_unlocked.as_mut() { - dac.ch1().set(val); - } -} - -async fn drive_motor( - lock_mode: bool, - duration_ms: u64, - force: bool, - current: u16, - ring_position: RingPosition, -) { - let mut deploy1_unlocked = DEPLOY_1_MTX.lock().await; - let mut deploy2_unlocked = DEPLOY_2_MTX.lock().await; - let mut motor_ps_unlocked = MOTOR_PS_MTX.lock().await; - if let Some(deploy1) = deploy1_unlocked.as_mut() { - if let Some(deploy2) = deploy2_unlocked.as_mut() { - if let Some(motor_ps) = motor_ps_unlocked.as_mut() { - deploy1.set_low(); - deploy2.set_low(); - motor_ps.set_high(); - Timer::after_millis(50).await; - let max_delay: u16 = 200; - - if lock_mode { - deploy2.set_high(); - limit_motor_current(current).await; - Timer::after_millis(duration_ms).await; - if force { - for _i in 0..max_delay { - Timer::after_millis(1).await; - if let RingPosition::Locked = ring_position { - deploy2.set_low(); - motor_ps.set_high(); - } - } - } - } else { - deploy1.set_high(); - limit_motor_current(current).await; - Timer::after_millis(duration_ms).await; - if force { - for _i in 0..max_delay { - Timer::after_millis(1).await; - if let RingPosition::Unlocked = ring_position { - deploy1.set_low(); - motor_ps.set_high(); - } - } - } - } - } - } - } -} - -#[embassy_executor::task] -pub async fn read_hall_sensor(mut sensor1: Peri<'static, PA0>, mut sensor2: Peri<'static, PA1>) { - - loop { - let sensor1_limits = SensorLimits::new(3100, 600, 1600, 700); - let sensor2_limits = SensorLimits::new(3100, 600, 2600, 700); - - let mut adc_unlocked = ADC_MTX.lock().await; - if let Some(adc) = adc_unlocked.as_mut() { - let sensor1_read = adc.read(&mut sensor1).await; - let sensor2_read = adc.read(&mut sensor2).await; - debug!("Sensor 1: {}, Sensor 2: {}", sensor1_read, sensor2_read); - - let sensor1_state = get_sensor_state(sensor1_read, sensor1_limits); - let sensor2_state = get_sensor_state(sensor2_read, sensor2_limits); - - let ring_position = get_ring_position(sensor1_state, sensor2_state); - RING_POSITION_CHANNEL.send(ring_position).await; - } - } -} - -#[embassy_executor::task] -async fn can_reader(mut can_rx: CanRx<'static>, mut can: Can<'static>) -> () { - loop { - match can_rx.read().await { - Ok(envelope) => match envelope.frame.id() { - Id::Standard(id) if id.as_raw() == DROGUE_ID => { - drive_motor(false, 50, false, 1000, RingPosition::Unlocked).await; - } - Id::Standard(id) if id.as_raw() == MAIN_ID => { - drive_motor(false, 50, false, 1000, RingPosition::Unlocked).await; - } - _ => {} - }, - - Err(e) => { - error!("CAN Read Error: {}", e); - can.sleep().await; - } - } - } -} diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/bin/parachute.rs b/controlSystem/RecoveryBoard/firmware-rs/src/bin/parachute.rs new file mode 100644 index 0000000..2fa907f --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/bin/parachute.rs @@ -0,0 +1,693 @@ +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_stm32::{ + adc::{Adc, InterruptHandler, SampleTime}, + bind_interrupts, + can::{ + filter::Mask32, frame::Header, Can, CanRx, Fifo, Frame, Id, Rx0InterruptHandler, + Rx1InterruptHandler, SceInterruptHandler, StandardId, TxInterruptHandler, + }, + dac::Dac, + flash::Flash, + gpio::{Input, Level, Output, OutputType, Pull, Speed}, + peripherals::{ADC1, CAN, USART2}, + time::Hertz, + timer::{ + low_level::CountingMode, + simple_pwm::{PwmPin, SimplePwm}, + }, + usart::{ + BufferedInterruptHandler, BufferedUart, Config as UartConfig, DataBits, Parity, StopBits, + }, +}; +use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex}; +use embassy_time::{with_timeout, Duration, Instant, TimeoutError, Timer}; +use embedded_io_async::{Read, Write}; +use firmware_rs::{ + adc::{read_battery_from_ref, ADC_MTX, BATT_READ_WATCH}, + blink::blink_led, + buzzer::{active_beep, BuzzerMode, BUZZER_MODE_MTX}, + can::{ + can_writer, CanTxChannelMsg, CAN_BITRATE, CAN_MTX, CAN_TX_CHANNEL, DROGUE_ACKNOWLEDGE_ID, + DROGUE_DEPLOY_ID, MAIN_ACKNOWLEDGE_ID, MAIN_DEPLOY_ID, SENDER_HEARTBEAT_ID, + }, + flash::{FLASH_MTX, MOTOR_ACT_SECTOR_OFFSET, MOTOR_ACT_SECTOR_SIZE}, + motor::{Motor, MotorType, MOTOR_DRIVE_CURR_MA, MOTOR_DRIVE_DUR_MS}, + ring::{read_pos_sensor, Ring, RingPosition, RING_MTX, RING_POSITION_WATCH, SENSOR_READ_WATCH}, + types::*, + uart::{IO, UART_BUF_SIZE, UART_RX_BUF_CELL, UART_TX_BUF_CELL}, +}; +use noline::builder::EditorBuilder; +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct CanIrqs { + CEC_CAN => + Rx0InterruptHandler, + Rx1InterruptHandler, + SceInterruptHandler, + TxInterruptHandler; +}); +bind_interrupts!(struct AdcIrqs { ADC1_COMP => InterruptHandler; }); +bind_interrupts!(struct UsartIrqs { USART2 => BufferedInterruptHandler; }); + +#[derive(Default)] +pub struct ChuteState { + pub id: u8, + pub ready: bool, + pub shore_power_status: bool, + pub sender_last_seen: u64, +} + +#[derive(Debug)] +pub enum ChuteStateField { + Id(u8), + Ready(bool), + ShorePowerStatus(bool), + SenderLastSeen(u64), +} + +pub struct ChuteStateIter<'a> { + state_fields: &'a ChuteState, + index: usize, +} + +impl ChuteState { + pub fn iter(&self) -> ChuteStateIter<'_> { + ChuteStateIter { state_fields: self, index: 0 } + } +} + +impl<'a> Iterator for ChuteStateIter<'a> { + type Item = ChuteStateField; + + fn next(&mut self) -> Option { + let result = match self.index { + 0 => Some(ChuteStateField::Id(self.state_fields.id)), + 1 => Some(ChuteStateField::Ready(self.state_fields.ready)), + 2 => Some(ChuteStateField::ShorePowerStatus(self.state_fields.shore_power_status)), + 3 => Some(ChuteStateField::SenderLastSeen(self.state_fields.sender_last_seen)), + _ => None, + }; + + if result.is_some() { + self.index += 1; + } + + result + } +} + +impl core::fmt::Display for ChuteStateField { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match *self { + Self::Id(val) => { + core::write!(f, "Id: {}", val) + } + Self::Ready(val) => { + core::write!(f, "Ready: {}", if val { "YES" } else { "NO" }) + } + Self::ShorePowerStatus(val) => { + core::write!(f, "Shore Power: {}", if val { "ON" } else { "OFF" }) + } + Self::SenderLastSeen(val) => core::write!(f, "Sender last seen: {}ms", val), + } + } +} + +async fn set_state(update: ChuteStateField) { + let mut unlocked = SYSTEM_STATE_MTX.lock().await; + if let Some(state) = unlocked.as_mut() { + match update { + ChuteStateField::Id(val) => state.id = val, + ChuteStateField::Ready(val) => state.ready = val, + ChuteStateField::ShorePowerStatus(val) => state.shore_power_status = val, + ChuteStateField::SenderLastSeen(val) => state.sender_last_seen = val, + } + } +} + +static UMB_ON_MTX: UmbOnType = Mutex::new(None); +static SYSTEM_STATE_MTX: Mutex> = Mutex::new(None); +static MOTOR_MTX: MotorType = Mutex::new(None); + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + let p = embassy_stm32::init(Default::default()); + + #[cfg(main)] + { + info!("main") + } + + #[cfg(drogue)] + { + info!("drogue") + } + + let umb_on = Input::new(p.PA8, Pull::Up); + let _can_shdn = Output::new(p.PA10, Level::Low, Speed::Medium); + let _can_silent = Output::new(p.PA9, Level::Low, Speed::Medium); + + // Set up PWM driver + let buzz_pin = PwmPin::new(p.PB15, OutputType::PushPull); + let pwm = SimplePwm::new( + p.TIM15, + None, + Some(buzz_pin), + None, + None, + Hertz(440), + CountingMode::EdgeAlignedUp, + ); + + let buzzer_mode = BuzzerMode::Off; + + // Set up CAN driver + let mut can = Can::new(p.CAN, p.PA11, p.PA12, CanIrqs); + can.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all()); + can.modify_config().set_bitrate(CAN_BITRATE).set_loopback(false).set_silent(false); + + // Set up ADC driver + let mut adc = Adc::new(p.ADC1, AdcIrqs); + adc.set_sample_time(SampleTime::CYCLES239_5); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + + // Set up UART driver + let mut uart_config = UartConfig::default(); + uart_config.baudrate = 115200; + uart_config.parity = Parity::ParityNone; + uart_config.data_bits = DataBits::DataBits8; + uart_config.stop_bits = StopBits::STOP1; + let uart = BufferedUart::new( + p.USART2, + p.PA3, + p.PA2, + UART_TX_BUF_CELL.take(), + UART_RX_BUF_CELL.take(), + UsartIrqs, + uart_config, + ) + .expect("Uart Config Error"); + + let dac = Dac::new(p.DAC1, p.DMA1_CH3, p.DMA1_CH4, p.PA4, p.PA5); + let mut sys_state = ChuteState::default(); + + #[cfg(drogue)] + { + sys_state.id = 1; + } + + #[cfg(main)] + { + sys_state.id = 2; + } + + let flash = Flash::new_blocking(p.FLASH); + + let motor = Motor::new(p.PB4, p.PB5, p.PB6, p.PB7, dac); + let ring = Ring::new(p.PA0, p.PA1, p.PB1); + + { + // Put peripherals into mutex if shared among tasks. + // Inner scope so that mutex is unlocked when out of scope + *(BUZZER_MODE_MTX.lock().await) = Some(buzzer_mode); + *(ADC_MTX.lock().await) = Some(adc); + *(UMB_ON_MTX.lock().await) = Some(umb_on); + *(SYSTEM_STATE_MTX.lock().await) = Some(sys_state); + *(RING_MTX.lock().await) = Some(ring); + *(MOTOR_MTX.lock().await) = Some(motor); + *(FLASH_MTX.lock().await) = Some(flash); + } + + unwrap!(spawner.spawn(blink_led(p.PB14))); + unwrap!(spawner.spawn(active_beep(pwm))); + unwrap!(spawner.spawn(cli(uart))); + unwrap!(spawner.spawn(read_battery_from_ref(p.PB0))); + unwrap!(spawner.spawn(read_pos_sensor())); + + // enable can at last minute so other tasks can still spawn if can bus is down + can.enable().await; + let (can_tx, can_rx) = can.split(); + + { + *(CAN_MTX.lock().await) = Some(can); + } + + unwrap!(spawner.spawn(can_writer(can_tx))); + unwrap!(spawner.spawn(can_reader(can_rx))); + unwrap!(spawner.spawn(parachute_heartbeat())); + + // Keep main from returning. Needed for can_tx/can_rx or they get dropped + core::future::pending::<()>().await; +} + +#[embassy_executor::task] +pub async fn cli(uart: BufferedUart<'static>) { + let prompt = "> "; + let mut io = IO::new(uart); + let mut buffer = [0; UART_BUF_SIZE]; + let mut history = [0; UART_BUF_SIZE]; + loop { + let mut editor = EditorBuilder::from_slice(&mut buffer) + .with_slice_history(&mut history) + .build_async(&mut io) + .await + .unwrap(); + + while let Ok(line) = editor.readline(prompt, &mut io).await { + // WARN: Passing more than 5 args panics + let args: heapless::Vec<&str, 5> = line.split_whitespace().collect(); + + if args.len() > 3usize { + error!("Only two arguments allowed"); + continue; + } + + if args.is_empty() { + error!("args empty"); + continue; + } + + match args[0] { + "help" => { + let lines = [ + "help: Display this message.\r\n\n", + "state: Print internal state.\r\n\n", + "l: Move the ring towards the lock position.\r\n", + " --force: Ignore sensor readings, continue until timeout.\r\n", + " --pulse: Do a 100ms step instead of a full swing.\r\n\n", + "u: Move the ring towards the unlocked position.\r\n", + " --force: Ignore sensor readings, continue until timeout.\r\n", + " --pulse: Do a 100ms step instead of a full swing.\r\n\n", + "pos: Print the current sensor readings and ring state.\r\n", + " --poll: Print the sensor value and ring state every second.\r\n\n", + "acts: Print the number of motor actuations stored in flash\r\n\n", + "erase: Erase motor actuation count data from flash\r\n\n", + ]; + for line in lines { + io.write(line.as_bytes()).await.unwrap(); + } + io.flush().await.unwrap(); + } + "state" => { + let mut buf = [0u8; 64]; + let mut state_unlocked = SYSTEM_STATE_MTX.lock().await; + if let Some(state) = state_unlocked.as_mut() { + let time_now = Instant::now().as_millis(); + let ts = format_no_std::show( + &mut buf, + format_args!("Current time: {}ms\r\n", time_now), + ) + .unwrap(); + io.write(ts.as_bytes()).await.unwrap(); + for field in state.iter() { + let s = format_no_std::show(&mut buf, format_args!("{}\r\n", field)) + .unwrap(); + io.write(s.as_bytes()).await.unwrap(); + } + } + } + "batt" => { + let mut buf = [0u8; 16]; + + let batt_read = BATT_READ_WATCH + .receiver() + .expect("Could not get batt_read receiver") + .changed() + .await; + + let s = + format_no_std::show(&mut buf, format_args!("{}\r\n", batt_read)).unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + "l" => { + let mut motor_unlocked = MOTOR_MTX.lock().await; + if let Some(motor) = motor_unlocked.as_mut() { + motor + .drive( + RingPosition::Locked, + if args.contains(&"--pulse") { + 100 + } else { + MOTOR_DRIVE_DUR_MS + }, + args.contains(&"--force"), + MOTOR_DRIVE_CURR_MA, + ) + .await; + } + } + "u" => { + let mut motor_unlocked = MOTOR_MTX.lock().await; + if let Some(motor) = motor_unlocked.as_mut() { + motor + .drive( + RingPosition::Unlocked, + if args.contains(&"--pulse") { + 100 + } else { + MOTOR_DRIVE_DUR_MS + }, + args.contains(&"--force"), + MOTOR_DRIVE_CURR_MA, + ) + .await; + } + } + "pos" => { + let mut wbuf = [0u8; 64]; + + let mut ring_pos_rcvr = RING_POSITION_WATCH + .receiver() + .expect("Could not get ring_pos receiver for pos cmd"); + let mut sensor_readings_rcvr = SENSOR_READ_WATCH + .receiver() + .expect("Could not get sensor readings receiver for pos cmd"); + + if args.contains(&"--poll") { + let mut buf = [0u8; 1]; + while let Err(TimeoutError) = + with_timeout(Duration::from_secs(1), io.read(&mut buf)).await + { + match ring_pos_rcvr.try_get() { + Some(RingPosition::Locked) => { + io.write(b"Ring Locked - ").await.unwrap(); + } + Some(RingPosition::Unlocked) => { + io.write(b"Ring Unlocked - ").await.unwrap(); + } + Some(RingPosition::Inbetween) => { + io.write(b"Ring Inbetween - ").await.unwrap(); + } + Some(RingPosition::Error) => { + io.write(b"Ring Error - ").await.unwrap(); + } + None => { + io.write(b"Ring Not Initialized - ").await.unwrap(); + } + } + + if let Some(sensor_readings) = sensor_readings_rcvr.try_get() { + let s = format_no_std::show( + &mut wbuf, + format_args!( + "Sensor 1: {} Sensor 2: {}\r\n", + sensor_readings.sensor1, sensor_readings.sensor2 + ), + ) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + } + } else { + match ring_pos_rcvr.try_get() { + Some(RingPosition::Locked) => { + io.write(b"Ring Locked - ").await.unwrap(); + } + Some(RingPosition::Unlocked) => { + io.write(b"Ring Unlocked - ").await.unwrap(); + } + Some(RingPosition::Inbetween) => { + io.write(b"Ring Inbetween - ").await.unwrap(); + } + Some(RingPosition::Error) => { + io.write(b"Ring Error - ").await.unwrap(); + } + None => { + io.write(b"Ring Not Initialized - \r\n").await.unwrap(); + } + } + + if let Some(sensor_readings) = sensor_readings_rcvr.try_get() { + let s = format_no_std::show( + &mut wbuf, + format_args!( + "Sensor 1: {} Sensor 2: {}\r\n", + sensor_readings.sensor1, sensor_readings.sensor2 + ), + ) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + } + } + "beep" => { + let mut buzzer_mode_unlocked = BUZZER_MODE_MTX.lock().await; + if let Some(mode) = buzzer_mode_unlocked.as_mut() { + match mode { + BuzzerMode::Off => { + *mode = BuzzerMode::Low; + } + _ => { + *mode = BuzzerMode::Off; + } + } + } + } + "erase" => { + let mut motor_unlocked = MOTOR_MTX.lock().await; + if let Some(motor) = motor_unlocked.as_mut() { + motor.erase_actuation_data().await; + } + } + "acts" => { + let mut flash_unlocked = FLASH_MTX.lock().await; + if let Some(flash) = flash_unlocked.as_mut() { + let mut bytes = [0u8; (MOTOR_ACT_SECTOR_SIZE / 8) as usize]; + let mut buf = [0u8; 64]; + + if let Err(e) = flash.blocking_read(MOTOR_ACT_SECTOR_OFFSET, &mut bytes) { + error!("Error reading flash: {}", e); + let s = format_no_std::show( + &mut buf, + format_args!("Error reading flash: {:?}\r\n", e), + ) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + + let s = format_no_std::show( + &mut buf, + format_args!("Actuations: {}\r\n", bytes[0] + 1), // Data starts at 255 + ) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + } + "version" => { + let mut buf = [0u8; 8]; + let version_details = env!("CARGO_PKG_VERSION"); + + let s = format_no_std::show(&mut buf, format_args!("{}\r\n", version_details)) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); + } + _ => { + io.write(b"Invalid command\r\n").await.unwrap(); + } + } + } + } +} + +#[embassy_executor::task] +async fn can_reader(mut can_rx: CanRx<'static>) -> () { + loop { + match can_rx.read().await { + Ok(envelope) => match envelope.frame.id() { + Id::Standard(id) if id.as_raw() == DROGUE_DEPLOY_ID => { + #[cfg(drogue)] + { + let frame = + Frame::new_data(StandardId::new(DROGUE_ACKNOWLEDGE_ID).unwrap(), &[1]) + .unwrap(); + let acknowledge_msg = CanTxChannelMsg::new(true, frame); + CAN_TX_CHANNEL.send(acknowledge_msg).await; + + let mut motor_unlocked = MOTOR_MTX.lock().await; + if let Some(motor) = motor_unlocked.as_mut() { + motor + .drive( + RingPosition::Unlocked, + MOTOR_DRIVE_DUR_MS, + false, + MOTOR_DRIVE_CURR_MA, + ) + .await; + } + } + } + Id::Standard(id) if id.as_raw() == MAIN_DEPLOY_ID => { + #[cfg(main)] + { + let frame = + Frame::new_data(StandardId::new(MAIN_ACKNOWLEDGE_ID).unwrap(), &[1]) + .unwrap(); + let acknowledge_msg = CanTxChannelMsg::new(true, frame); + CAN_TX_CHANNEL.send(acknowledge_msg).await; + { + let mut motor_unlocked = MOTOR_MTX.lock().await; + if let Some(motor) = motor_unlocked.as_mut() { + motor + .drive( + RingPosition::Unlocked, + MOTOR_DRIVE_DUR_MS, + false, + MOTOR_DRIVE_CURR_MA, + ) + .await; + } + } + } + } + Id::Standard(id) if id.as_raw() == SENDER_HEARTBEAT_ID => { + set_state(ChuteStateField::SenderLastSeen(envelope.ts.as_millis())).await; + } + _ => {} + }, + + Err(e) => { + error!("CAN Read Error: {}", e); + let mut can_unlocked = CAN_MTX.lock().await; + if let Some(can) = can_unlocked.as_mut() { + can.sleep().await; + } + } + } + } +} + +#[embassy_executor::task] +async fn parachute_heartbeat() -> () { + let mut prev_ready: u8 = 0; + let mut prev_batt_ok: u8 = 0; + let mut prev_sender_status: u8 = 0; + let mut prev_shore_pow_status: u8 = 0; + + let mut ring_pos_rcvr = + RING_POSITION_WATCH.receiver().expect("Could not get ring pos receiver for heartbeat task"); + + let mut batt_read_rcvr = + BATT_READ_WATCH.receiver().expect("Could not get batt read receiver for heartbeat task"); + + loop { + let time_now = Instant::now().as_millis(); + let sender_last_seen: u64; + + { + let mut state_unlocked = SYSTEM_STATE_MTX.lock().await; + if let Some(state) = state_unlocked.as_mut() { + sender_last_seen = state.sender_last_seen; + } else { + error!("Could not read state for heartbeat"); + continue; + } + } + + let ring_pos_u8: u8 = match ring_pos_rcvr.try_get() { + Some(RingPosition::Unlocked) => 1, + Some(RingPosition::Inbetween) => 2, + Some(RingPosition::Locked) => 3, + Some(RingPosition::Error) => 4, + None => 0, + }; + + let batt_read = batt_read_rcvr.changed().await; + let batt_ok: u8 = (batt_read > 99) as u8; + + let sender_status: u8 = ((time_now - sender_last_seen) < 2000) as u8; + + { + let mut umb_on_unlocked = UMB_ON_MTX.lock().await; + if let Some(umb_on_ref) = umb_on_unlocked.as_mut() { + let shore_pow_status = umb_on_ref.is_low() as u8; + + set_state(ChuteStateField::ShorePowerStatus(shore_pow_status == 1)).await; + + let ready = (ring_pos_u8 == 2 + && shore_pow_status == 0 + && batt_ok == 1 + && sender_status == 1) as u8; + + set_state(ChuteStateField::Ready(ready == 1)).await; + + { + let mut buzz_mode_unlocked = BUZZER_MODE_MTX.lock().await; + if let Some(mode) = buzz_mode_unlocked.as_mut() { + match mode { + BuzzerMode::Off => {} + _ => { + if ready > 0 { + *mode = BuzzerMode::High; + } else { + *mode = BuzzerMode::Low; + } + } + } + } + } + + let status_buf = [ + ring_pos_u8, + batt_read, + batt_ok, + shore_pow_status, + sender_status, + ready, + 0, + 0, + ]; + + #[cfg(main)] + { + use firmware_rs::can::MAIN_HEARTBEAT_ID; + let id = StandardId::new(MAIN_HEARTBEAT_ID).unwrap(); + let header = Header::new(Id::Standard(id), 8, false); + let frame = Frame::new(header, &status_buf).unwrap(); + let msg: CanTxChannelMsg = CanTxChannelMsg::new(false, frame); + CAN_TX_CHANNEL.send(msg).await; + } + + #[cfg(drogue)] + { + use firmware_rs::can::DROGUE_HEARTBEAT_ID; + let id = StandardId::new(DROGUE_HEARTBEAT_ID).unwrap(); + let header = Header::new(Id::Standard(id), 8, false); + let frame = Frame::new(header, &status_buf).unwrap(); + let msg: CanTxChannelMsg = CanTxChannelMsg::new(false, frame); + CAN_TX_CHANNEL.send(msg).await; + } + + if batt_ok != prev_batt_ok { + info!("Battery ok changed to {}", batt_ok); + } + + if ready != prev_ready { + info!("Ready status changed to {}", ready); + } + + if shore_pow_status != prev_shore_pow_status { + info!("Shore power status changed to {}", shore_pow_status); + } + if sender_status != prev_sender_status { + info!("Sender status changed to {}", sender_status); + } + + prev_sender_status = sender_status; + prev_shore_pow_status = shore_pow_status; + prev_ready = ready; + prev_batt_ok = batt_ok; + } + } + Timer::after_secs(1).await; + } +} diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/bin/sender.rs b/controlSystem/RecoveryBoard/firmware-rs/src/bin/sender.rs index 7d4fc50..100e54f 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/bin/sender.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/bin/sender.rs @@ -31,12 +31,14 @@ use embassy_stm32::{ use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex}; use embassy_time::{Instant, Timer}; use embedded_io_async::Write; -use firmware_rs::shared::{ - adc::{read_battery, BATT_READ_CHANNEL}, - buzzer::{active_beep, BuzzerMode, BuzzerModeMtxType}, +use firmware_rs::{ + adc::{read_battery, BATT_READ_WATCH}, + blink::blink_led, + buzzer::{active_beep, BuzzerMode, BUZZER_MODE_MTX}, can::{ - can_writer, CanTxChannelMsg, CAN_BITRATE, CAN_TX_CHANNEL, DROGUE_ACKNOWLEDGE_ID, DROGUE_ID, - DROGUE_STATUS_ID, MAIN_ACKNOWLEDGE_ID, MAIN_ID, MAIN_STATUS_ID, TELEMETRUM_HEARTBEAT_ID, + can_writer, CanTxChannelMsg, CAN_BITRATE, CAN_MTX, CAN_TX_CHANNEL, DROGUE_ACKNOWLEDGE_ID, + DROGUE_DEPLOY_ID, DROGUE_HEARTBEAT_ID, MAIN_ACKNOWLEDGE_ID, MAIN_DEPLOY_ID, MAIN_HEARTBEAT_ID, + SENDER_HEARTBEAT_ID, }, types::*, uart::{IO, UART_BUF_SIZE, UART_RX_BUF_CELL, UART_TX_BUF_CELL}, @@ -56,7 +58,6 @@ bind_interrupts!(struct UsartIrqs { USART2 => BufferedInterruptHandler; static UMB_ON_MTX: UmbOnType = Mutex::new(None); static SYSTEM_STATE_MTX: Mutex> = Mutex::new(None); -static BUZZER_MODE_MTX: BuzzerModeMtxType = Mutex::new(None); static MAIN_ACKNOWLEDGE: AtomicBool = AtomicBool::new(false); static DROGUE_ACKNOWLEDGE: AtomicBool = AtomicBool::new(false); @@ -67,7 +68,7 @@ pub struct SenderState { pub force_rocket_ready: bool, pub drogue_status: bool, pub main_status: bool, - pub shore_power_status: bool, + pub shore_power_status: bool, pub drogue_last_seen: u64, pub main_last_seen: u64, pub iso_main_last_seen: u64, @@ -189,7 +190,7 @@ async fn main(spawner: Spawner) { CountingMode::EdgeAlignedUp, ); - let buzzer_mode = BuzzerMode::Low; + let buzzer_mode = BuzzerMode::Off; // Set up CAN driver let mut can = Can::new(p.CAN, p.PA11, p.PA12, CanIrqs); @@ -228,18 +229,24 @@ async fn main(spawner: Spawner) { *(SYSTEM_STATE_MTX.lock().await) = Some(sys_state); } - unwrap!(spawner.spawn(active_beep(pwm, &BUZZER_MODE_MTX))); + unwrap!(spawner.spawn(blink_led(p.PB14))); + unwrap!(spawner.spawn(active_beep(pwm))); unwrap!(spawner.spawn(cli(uart))); unwrap!(spawner.spawn(read_battery(adc, p.PB0))); - unwrap!(spawner.spawn(handle_iso_rising_edge(iso_drogue, DROGUE_ID))); - unwrap!(spawner.spawn(handle_iso_rising_edge(iso_main, MAIN_ID))); + unwrap!(spawner.spawn(handle_iso_rising_edge(iso_drogue, DROGUE_DEPLOY_ID))); + unwrap!(spawner.spawn(handle_iso_rising_edge(iso_main, MAIN_DEPLOY_ID))); unwrap!(spawner.spawn(telemetrum_heartbeat(rocket_ready_pin))); // enable at last minute so other tasks can still spawn if can bus is down can.enable().await; let (can_tx, can_rx) = can.split(); - unwrap!(spawner.spawn(can_writer(can_tx, &CAN_TX_CHANNEL))); - unwrap!(spawner.spawn(can_reader(can_rx, can))); + + { + *(CAN_MTX.lock().await) = Some(can); + } + + unwrap!(spawner.spawn(can_writer(can_tx))); + unwrap!(spawner.spawn(can_reader(can_rx))); // Keep main from returning. Needed for can_tx/can_rx or they get dropped core::future::pending::<()>().await; @@ -251,7 +258,7 @@ async fn deploy(can_id: u16) { let frame = unwrap!(Frame::new(header, &[1; 0])); match can_id { - DROGUE_ID => { + DROGUE_DEPLOY_ID => { info!("Releasing drogue"); while !DROGUE_ACKNOWLEDGE.load(core::sync::atomic::Ordering::Relaxed) { let msg = CanTxChannelMsg::new(true, frame); @@ -261,7 +268,7 @@ async fn deploy(can_id: u16) { info!("Drogue release acknowledged"); DROGUE_ACKNOWLEDGE.store(false, core::sync::atomic::Ordering::Relaxed); } - MAIN_ID => { + MAIN_DEPLOY_ID => { info!("Releasing main"); while !MAIN_ACKNOWLEDGE.load(core::sync::atomic::Ordering::Relaxed) { let msg = CanTxChannelMsg::new(true, frame); @@ -326,10 +333,10 @@ async fn cli(uart: BufferedUart<'static>) { } } "drogue" => { - deploy(DROGUE_ID).await; + deploy(DROGUE_DEPLOY_ID).await; } "main" => { - deploy(MAIN_ID).await; + deploy(MAIN_DEPLOY_ID).await; } "rr" => { let toggled_rr: bool; @@ -344,12 +351,17 @@ async fn cli(uart: BufferedUart<'static>) { set_state(SenderStateField::ForceRocketReady(toggled_rr)).await; } "batt" => { - // WARN: This will cause the CAN bus to wait for the next batt read to send - // telemetrum heartbeat let mut buf = [0u8; 16]; - let batt_read = BATT_READ_CHANNEL.receive().await; + + let batt_read = BATT_READ_WATCH + .receiver() + .expect("Could not get batt_read receiver") + .changed() + .await; + let s = format_no_std::show(&mut buf, format_args!("{}\r\n", batt_read)).unwrap(); + io.write(s.as_bytes()).await.unwrap(); } "beep" => { @@ -369,7 +381,13 @@ async fn cli(uart: BufferedUart<'static>) { } } "version" => { - // TODO: implement - print version details + let mut buf = [0u8; 8]; + let version_details = env!("CARGO_PKG_VERSION"); + + let s = format_no_std::show(&mut buf, format_args!("{}\r\n", version_details)) + .unwrap(); + + io.write(s.as_bytes()).await.unwrap(); } _ => { io.write(b"Invalid Command\r\n").await.unwrap(); @@ -380,13 +398,13 @@ async fn cli(uart: BufferedUart<'static>) { } #[embassy_executor::task] -async fn can_reader(mut can_rx: CanRx<'static>, mut can: Can<'static>) -> () { +async fn can_reader(mut can_rx: CanRx<'static>) -> () { let mut prev_main_status: u8 = 0; let mut prev_drogue_status: u8 = 0; loop { match can_rx.read().await { Ok(envelope) => match envelope.frame.id() { - Id::Standard(id) if id.as_raw() == MAIN_STATUS_ID => { + Id::Standard(id) if id.as_raw() == MAIN_HEARTBEAT_ID => { let status = envelope.frame.data()[0]; set_state(SenderStateField::MainLastSeen(envelope.ts.as_millis())).await; set_state(SenderStateField::MainStatus(status > 0)).await; @@ -398,7 +416,7 @@ async fn can_reader(mut can_rx: CanRx<'static>, mut can: Can<'static>) -> () { Id::Standard(id) if id.as_raw() == MAIN_ACKNOWLEDGE_ID => { MAIN_ACKNOWLEDGE.store(true, core::sync::atomic::Ordering::Relaxed); } - Id::Standard(id) if id.as_raw() == DROGUE_STATUS_ID => { + Id::Standard(id) if id.as_raw() == DROGUE_HEARTBEAT_ID => { let status = envelope.frame.data()[0]; set_state(SenderStateField::DrogueLastSeen(envelope.ts.as_millis())).await; set_state(SenderStateField::DrogueStatus(status > 0)).await; @@ -414,7 +432,10 @@ async fn can_reader(mut can_rx: CanRx<'static>, mut can: Can<'static>) -> () { }, Err(e) => { error!("CAN Read Error: {}", e); - can.sleep().await; + let mut can_unlocked = CAN_MTX.lock().await; + if let Some(can) = can_unlocked.as_mut() { + can.sleep().await; + } } } } @@ -436,10 +457,10 @@ async fn handle_iso_rising_edge(mut iso: ExtiInput<'static>, can_id: u16) -> () } // record last deployment signal time match can_id { - DROGUE_ID => { + DROGUE_DEPLOY_ID => { set_state(SenderStateField::IsoDrogueLastSeen(time_now)).await; } - MAIN_ID => { + MAIN_DEPLOY_ID => { set_state(SenderStateField::IsoMainLastSeen(time_now)).await; } _ => {} @@ -496,7 +517,10 @@ async fn telemetrum_heartbeat(mut rr_pin: Output<'static>) -> () { let main_ok = time_now - main_last_seen < 2000; let drogue_ok = time_now - drogue_last_seen < 2000; - let batt_read = BATT_READ_CHANNEL.receive().await; + + let batt_read = + BATT_READ_WATCH.receiver().expect("Could not get batt_read receiver").changed().await; + let batt_ok = (batt_read > 99) as u8; let can_bus_ok = (main_ok && drogue_ok) as u8; let ers_status = (can_bus_ok != 0 && main_status && drogue_status) as u8; @@ -545,7 +569,7 @@ async fn telemetrum_heartbeat(mut rr_pin: Output<'static>) -> () { 0, ]; - let id = StandardId::new(TELEMETRUM_HEARTBEAT_ID).unwrap(); + let id = StandardId::new(SENDER_HEARTBEAT_ID).unwrap(); let header = Header::new(Standard(id), 8, false); let frame = Frame::new(header, &status_buf).unwrap(); diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/blink.rs b/controlSystem/RecoveryBoard/firmware-rs/src/blink.rs new file mode 100644 index 0000000..3c5bf82 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/blink.rs @@ -0,0 +1,17 @@ +use embassy_stm32::{ + gpio::Output, peripherals::PB14, Peri +}; +use embassy_time::Timer; + +#[embassy_executor::task] +pub async fn blink_led(led_pin: Peri<'static, PB14>) { + let mut led = + Output::new(led_pin, embassy_stm32::gpio::Level::High, embassy_stm32::gpio::Speed::Medium); + loop { + led.set_high(); + Timer::after_millis(300).await; + + led.set_low(); + Timer::after_millis(300).await; + } +} diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/buzzer.rs b/controlSystem/RecoveryBoard/firmware-rs/src/buzzer.rs similarity index 90% rename from controlSystem/RecoveryBoard/firmware-rs/src/shared/buzzer.rs rename to controlSystem/RecoveryBoard/firmware-rs/src/buzzer.rs index 4705d80..eba5de8 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/shared/buzzer.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/buzzer.rs @@ -1,4 +1,3 @@ -use defmt::info; use embassy_stm32::{peripherals::TIM15, time::Hertz, timer::simple_pwm::SimplePwm}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; @@ -11,8 +10,10 @@ pub enum BuzzerMode { pub type BuzzerModeMtxType = Mutex>; +pub static BUZZER_MODE_MTX: BuzzerModeMtxType = Mutex::new(None); + #[embassy_executor::task] -pub async fn active_beep(mut pwm: SimplePwm<'static, TIM15>, mode: &'static BuzzerModeMtxType) { +pub async fn active_beep(mut pwm: SimplePwm<'static, TIM15>) { // start up melody pwm.ch2().enable(); @@ -31,7 +32,7 @@ pub async fn active_beep(mut pwm: SimplePwm<'static, TIM15>, mode: &'static Buzz loop { let mut delay: u64 = 777; // Picking delays that hopefully won't overlap other tasks { - let mut mode_unlocked = mode.lock().await; + let mut mode_unlocked = BUZZER_MODE_MTX.lock().await; if let Some(mode) = mode_unlocked.as_mut() { match mode { BuzzerMode::High => { diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/can.rs b/controlSystem/RecoveryBoard/firmware-rs/src/can.rs similarity index 62% rename from controlSystem/RecoveryBoard/firmware-rs/src/shared/can.rs rename to controlSystem/RecoveryBoard/firmware-rs/src/can.rs index 7edfbdd..0132148 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/shared/can.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/can.rs @@ -1,18 +1,23 @@ use defmt::*; use embassy_stm32::can::{Can, CanTx, Frame, StandardId}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel}; +use embassy_sync::{ + blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, + channel::Channel, + mutex::Mutex, +}; use embassy_time::Timer; pub const CAN_BITRATE: u32 = 1_000_000; -pub const DROGUE_ID: u16 = 0x100; -pub const DROGUE_STATUS_ID: u16 = 0x710; +pub const DROGUE_DEPLOY_ID: u16 = 0x100; +pub const DROGUE_HEARTBEAT_ID: u16 = 0x710; pub const DROGUE_ACKNOWLEDGE_ID: u16 = 0x101; -pub const MAIN_ID: u16 = 0x200; -pub const MAIN_STATUS_ID: u16 = 0x720; +pub const MAIN_DEPLOY_ID: u16 = 0x200; +pub const MAIN_HEARTBEAT_ID: u16 = 0x720; pub const MAIN_ACKNOWLEDGE_ID: u16 = 0x201; -pub const TELEMETRUM_HEARTBEAT_ID: u16 = 0x700; +pub const SENDER_HEARTBEAT_ID: u16 = 0x700; pub static CAN_TX_CHANNEL: Channel = Channel::new(); +pub static CAN_MTX: Mutex> = Mutex::new(None); pub struct CanTxChannelMsg { pub blocking: bool, @@ -39,17 +44,21 @@ pub async fn echo_can(mut can: Can<'static>) -> () { #[embassy_executor::task] pub async fn can_writer( mut can_tx: CanTx<'static>, - can_tx_ch: &'static Channel, ) -> () { loop { - let frame = can_tx_ch.receive().await; - debug!("Sending CAN frame: {}", frame.frame); + let frame = CAN_TX_CHANNEL.receive().await; if frame.blocking { can_tx.write(&frame.frame).await; } if !frame.blocking { if let Err(e) = can_tx.try_write(&frame.frame) { error!("Could not send CAN message: {}", e); + let mut can_unlocked = CAN_MTX.lock().await; + if let Some(can) = can_unlocked.as_mut() { + // Try to recover from bus_off mode + can.modify_config(); + can.enable().await; + } }; } } diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/flash.rs b/controlSystem/RecoveryBoard/firmware-rs/src/flash.rs new file mode 100644 index 0000000..59da452 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/flash.rs @@ -0,0 +1,9 @@ +use embassy_sync::mutex::Mutex; + +use crate::types::FlashType; + +// Motor actuations count sector +pub const MOTOR_ACT_SECTOR_OFFSET: u32 = 0x3F000; // Secctor 64 Addr: 0x0803_F000 +pub const MOTOR_ACT_SECTOR_SIZE: u32 = 4096; // Sector size is 4Kbytes + +pub static FLASH_MTX: FlashType = Mutex::new(None); diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/lib.rs b/controlSystem/RecoveryBoard/firmware-rs/src/lib.rs index 8116cd1..0866944 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/lib.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/lib.rs @@ -1,4 +1,12 @@ #![no_std] #![no_main] -pub mod shared; +pub mod motor; +pub mod ring; +pub mod can; +pub mod buzzer; +pub mod types; +pub mod adc; +pub mod uart; +pub mod blink; +pub mod flash; diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/motor.rs b/controlSystem/RecoveryBoard/firmware-rs/src/motor.rs new file mode 100644 index 0000000..c9b3076 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/motor.rs @@ -0,0 +1,197 @@ +use defmt::*; +use embassy_stm32::{ + dac::{Dac, Value}, + gpio::{Input, Level, Output, Pull, Speed}, + mode::Async, + peripherals::{DAC1, PB4, PB5, PB6, PB7}, + Peri, +}; +use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex}; +use embassy_time::{with_timeout, Duration}; + +use crate::{ + flash::{FLASH_MTX, MOTOR_ACT_SECTOR_OFFSET, MOTOR_ACT_SECTOR_SIZE}, + ring::{RingPosition, MOTOR_ISENSE_WATCH, RING_POSITION_WATCH}, +}; + +pub const MOTOR_DRIVE_DUR_MS: u64 = 1000; +pub const MOTOR_DRIVE_CURR_MA: u16 = 1000; + +pub struct Motor { + pub deploy1: Output<'static>, + pub deploy2: Output<'static>, + pub ps: Output<'static>, + pub motor_fail: Input<'static>, + pub dac: Dac<'static, DAC1, Async>, +} + +pub enum MotorMode { + PowerSave, + Stop, + Forward, + Reverse, + Brake, +} + +impl Motor { + pub fn new( + pb4: Peri<'static, PB4>, + pb5: Peri<'static, PB5>, + pb6: Peri<'static, PB6>, + pb7: Peri<'static, PB7>, + dac: Dac<'static, DAC1, Async>, + ) -> Self { + let deploy1 = Output::new(pb4, Level::Low, Speed::Medium); + let deploy2 = Output::new(pb5, Level::Low, Speed::Medium); + let ps = Output::new(pb6, Level::High, Speed::Medium); + let motor_fail = Input::new(pb7, Pull::Up); + + Self { deploy1, deploy2, ps, motor_fail, dac } + } + + pub fn set_mode(&mut self, mode: MotorMode) { + match mode { + MotorMode::PowerSave => { + self.ps.set_low(); + self.deploy1.set_low(); + self.deploy2.set_low(); + } + MotorMode::Stop => { + self.ps.set_high(); + self.deploy1.set_low(); + self.deploy2.set_low(); + } + MotorMode::Forward => { + self.ps.set_high(); + self.deploy1.set_high(); + self.deploy2.set_low(); + } + MotorMode::Reverse => { + self.ps.set_high(); + self.deploy1.set_low(); + self.deploy2.set_high(); + } + MotorMode::Brake => { + self.ps.set_high(); + self.deploy1.set_high(); + self.deploy2.set_high(); + } + } + } + + async fn limit_motor_current(&mut self, ma: u16) { + if ma >= 2000 { + error!("Current too high!"); + return; + }; + // math is calculating the dac value from circuit analysis going backwards from motor current. + // First we do mA*3/10, which gives us MOTOR_VREF, then multiply by 2 to get MOTOR_ILIM and + // then lastly the dac takes in a 11 bit value which represents a scalar of the max voltage + // (3.3V). Which we use mV*3300/4096 to get the step size from the output voltage. + // Combining all the numerators and denominators to avoid overflow we get 1024/1375 + let scale = ((ma as u32) * 1024 / 1375) as u16; + let val = Value::Bit12Right(scale); + self.dac.ch1().set(val); + } + + async fn read_ring_pos_until_condition(&mut self, position: RingPosition) { + let mut ring_pos_receiver = + RING_POSITION_WATCH.receiver().expect("Could not get ring_pos rcvr"); + let mut isense_receiver = MOTOR_ISENSE_WATCH.receiver().expect("Could not get isense rcvr"); + const BUFSIZE: usize = 64; // INFO If running the motor for longer, increase this + let mut buf = [0u16; BUFSIZE]; + let mut count = 0usize; + loop { + if count < BUFSIZE { + buf[count] = isense_receiver.changed().await; + } + let ring_position = ring_pos_receiver.changed().await; + count = count.wrapping_add(1); + if ring_position == position { + break; + } + } + // INFO Only gets to this point if the ring reaches the desired position + debug!("Motor_isense: {}", buf[..count]); + } + + async fn increment_actuation_count(&self) { + let mut flash_unlocked = FLASH_MTX.lock().await; + if let Some(flash) = flash_unlocked.as_mut() { + let mut buf = [0u8; (MOTOR_ACT_SECTOR_SIZE / 8) as usize]; + + if let Err(e) = flash.blocking_read(MOTOR_ACT_SECTOR_OFFSET, &mut buf) { + error!("Error reading actuation count from memory: {}", e); + } + + buf[0] = buf[0].wrapping_add(1); + + // Sector must be erased before writing or SEQ err will be thrown + if let Err(e) = flash.blocking_erase( + MOTOR_ACT_SECTOR_OFFSET, + MOTOR_ACT_SECTOR_OFFSET + MOTOR_ACT_SECTOR_SIZE, + ) { + error!("Error erasing memory: {}", e); + } + + if let Err(e) = flash.blocking_write(MOTOR_ACT_SECTOR_OFFSET, &buf) { + error!("Error writing actuation count to memory: {}", e); + } + } + } + + pub async fn erase_actuation_data(&self) { + let mut flash_unlocked = FLASH_MTX.lock().await; + if let Some(flash) = flash_unlocked.as_mut() { + info!("Erasing motor actuation data"); + if let Err(e) = flash.blocking_erase( + MOTOR_ACT_SECTOR_OFFSET, + MOTOR_ACT_SECTOR_OFFSET + MOTOR_ACT_SECTOR_SIZE, + ) { + error!("Error erasing memory: {}", e); + } + info!("Motor actuation data erased"); + } + } + + pub async fn drive(&mut self, mode: RingPosition, duration_ms: u64, force: bool, current: u16) { + self.limit_motor_current(current).await; + self.set_mode(MotorMode::Stop); + match mode { + RingPosition::Locked => { + self.set_mode(MotorMode::Reverse); + if force { + let limit = core::future::pending::<()>(); + if let Err(e) = with_timeout(Duration::from_millis(duration_ms), limit).await { + error!("Motor limit timed out: {}", e); + } + } else { + let limit = self.read_ring_pos_until_condition(RingPosition::Locked); + if let Err(e) = with_timeout(Duration::from_millis(duration_ms), limit).await { + error!("Motor limit timed out: {}", e); + } + } + } + RingPosition::Unlocked => { + self.set_mode(MotorMode::Forward); + if force { + let limit = core::future::pending::<()>(); + if let Err(e) = with_timeout(Duration::from_millis(duration_ms), limit).await { + error!("Motor limit timed out: {}", e); + } + } else { + let limit = self.read_ring_pos_until_condition(RingPosition::Unlocked); + if let Err(e) = with_timeout(Duration::from_millis(duration_ms), limit).await { + error!("Motor limit timed out: {}", e); + } + } + } + _ => error!("Unhandled drive mode. Choose Locked or Unlocked"), + } + + self.set_mode(MotorMode::PowerSave); + self.increment_actuation_count().await; + } +} + +pub type MotorType = Mutex>; diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/ring.rs b/controlSystem/RecoveryBoard/firmware-rs/src/ring.rs new file mode 100644 index 0000000..d6e5107 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/src/ring.rs @@ -0,0 +1,159 @@ +use embassy_stm32::peripherals::PA0; +use embassy_stm32::peripherals::PA1; +use embassy_stm32::peripherals::PB1; +use embassy_stm32::Peri; +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +use embassy_sync::mutex::Mutex; +use embassy_sync::watch::Watch; +use embassy_time::Timer; + +use crate::adc::ADC_MTX; + +pub type RingType = Mutex>; + +pub static RING_MTX: RingType = Mutex::new(None); + +pub static RING_POSITION_WATCH: Watch = Watch::new(); +pub static SENSOR_READ_WATCH: Watch = Watch::new(); +pub static MOTOR_ISENSE_WATCH: Watch = Watch::new(); + +#[derive(defmt::Format, PartialEq, Clone)] +pub enum RingPosition { + Locked, + Unlocked, + Inbetween, + Error, +} + +#[derive(Clone)] +pub struct SensorReadings { + pub sensor1: u16, + pub sensor2: u16, +} + +impl SensorReadings { + pub fn new(sensor1: u16, sensor2: u16) -> Self { + Self { sensor1, sensor2 } + } +} + +#[derive(Default)] +struct SensorLimits { + over: u16, + under: u16, + active: u16, + unactive: u16, +} + +impl SensorLimits { + fn new(over: u16, under: u16, active: u16, unactive: u16) -> Self { + Self { over, under, active, unactive } + } +} + +#[derive(PartialEq)] +enum SensorState { + Active, + Unactive, + Under, + Over, + Inbetween, +} + +pub struct Ring { + pa0: Peri<'static, PA0>, + pa1: Peri<'static, PA1>, + pb1: Peri<'static, PB1>, + sensor1_limits: SensorLimits, + sensor2_limits: SensorLimits, +} + +impl Ring { + pub fn new(pa0: Peri<'static, PA0>, pa1: Peri<'static, PA1>, pb1: Peri<'static, PB1>) -> Self { + let sensor1_limits = SensorLimits::new(3700, 600, 2100, 900); + let sensor2_limits = SensorLimits::new(3700, 600, 1300, 900); + + Self { pa0, pa1, pb1, sensor1_limits, sensor2_limits } + } + + pub async fn broadcast_ring_position(&mut self) { + let ring_position_sender = RING_POSITION_WATCH.sender(); + let sensor_reading_sender = SENSOR_READ_WATCH.sender(); + let motor_isense_sender = MOTOR_ISENSE_WATCH.sender(); + + fn get_sensor_state(adc_val: u16, limit: &SensorLimits) -> SensorState { + if adc_val >= limit.over { + SensorState::Over + } else if adc_val <= limit.under { + SensorState::Under + } else if adc_val >= limit.active && adc_val < limit.over { + SensorState::Active + } else if adc_val <= limit.unactive && adc_val > limit.under { + SensorState::Unactive + } else { + SensorState::Inbetween + } + } + + fn get_ring_position( + sensor1_state: SensorState, + sensor2_state: SensorState, + ) -> RingPosition { + if sensor1_state == SensorState::Active && sensor2_state != SensorState::Active { + return RingPosition::Unlocked; + } + if sensor1_state != SensorState::Unactive && sensor2_state == SensorState::Unactive { + return RingPosition::Unlocked; + } + if sensor1_state != SensorState::Active && sensor2_state == SensorState::Active { + return RingPosition::Locked; + } + if sensor1_state == SensorState::Unactive && sensor2_state != SensorState::Unactive { + return RingPosition::Locked; + } + if sensor1_state == SensorState::Inbetween || sensor2_state == SensorState::Inbetween { + RingPosition::Inbetween + } else { + RingPosition::Error + } + } + + let mut sensor1_read = 0u16; + let mut sensor2_read = 0u16; + let mut motor_isense_read = 0u16; + + { + let mut adc_unlocked = ADC_MTX.lock().await; + if let Some(adc) = adc_unlocked.as_mut() { + sensor1_read = adc.read(&mut self.pa0).await; + sensor2_read = adc.read(&mut self.pa1).await; + motor_isense_read = adc.read(&mut self.pb1).await; + } + } + + let readings = SensorReadings::new(sensor1_read, sensor2_read); + + sensor_reading_sender.send(readings); + motor_isense_sender.send(motor_isense_read); + + let sensor1_state = get_sensor_state(sensor1_read, &self.sensor1_limits); + let sensor2_state = get_sensor_state(sensor2_read, &self.sensor2_limits); + + let ring_position = get_ring_position(sensor1_state, sensor2_state); + + ring_position_sender.send(ring_position); + } +} + +#[embassy_executor::task] +pub async fn read_pos_sensor() { + loop { + { + let mut ring_unlocked = RING_MTX.lock().await; + if let Some(ring) = ring_unlocked.as_mut() { + ring.broadcast_ring_position().await; + } + } + Timer::after_millis(50).await; + } +} diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/mod.rs b/controlSystem/RecoveryBoard/firmware-rs/src/shared/mod.rs deleted file mode 100644 index c29f817..0000000 --- a/controlSystem/RecoveryBoard/firmware-rs/src/shared/mod.rs +++ /dev/null @@ -1,5 +0,0 @@ -pub mod can; -pub mod buzzer; -pub mod types; -pub mod adc; -pub mod uart; diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/types.rs b/controlSystem/RecoveryBoard/firmware-rs/src/types.rs similarity index 81% rename from controlSystem/RecoveryBoard/firmware-rs/src/shared/types.rs rename to controlSystem/RecoveryBoard/firmware-rs/src/types.rs index f741604..9d3466f 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/src/shared/types.rs +++ b/controlSystem/RecoveryBoard/firmware-rs/src/types.rs @@ -2,6 +2,7 @@ use embassy_stm32::{ adc::Adc, can::Can, dac::Dac, + flash::{Blocking, Flash}, gpio::Input, mode::Async, peripherals::{ADC1, DAC1, TIM15}, @@ -10,7 +11,6 @@ use embassy_stm32::{ use embassy_sync::{ blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, mutex::Mutex, - signal::Signal, }; pub type PwmType = Mutex>>; @@ -18,5 +18,4 @@ pub type UmbOnType = Mutex>>; pub type CanType = Mutex>>; pub type AdcType = Mutex>>; pub type DacType = Mutex>>; -pub type BattOkSignalType = Signal; -pub type ShortPowOnSignalType = Signal; +pub type FlashType = Mutex>>; diff --git a/controlSystem/RecoveryBoard/firmware-rs/src/shared/uart.rs b/controlSystem/RecoveryBoard/firmware-rs/src/uart.rs similarity index 100% rename from controlSystem/RecoveryBoard/firmware-rs/src/shared/uart.rs rename to controlSystem/RecoveryBoard/firmware-rs/src/uart.rs diff --git a/controlSystem/RecoveryBoard/firmware-rs/test_can.py b/controlSystem/RecoveryBoard/firmware-rs/test_can.py index 8d5f840..47e8b35 100644 --- a/controlSystem/RecoveryBoard/firmware-rs/test_can.py +++ b/controlSystem/RecoveryBoard/firmware-rs/test_can.py @@ -1,89 +1,51 @@ -# Requires python-can v4.5, pySerial v3.5 +import time import can -import argparse -import logging -from typing import cast -logger = logging.getLogger(__name__) -logging.basicConfig( - format="%(asctime)s - [%(levelname)s]: %(message)s", - datefmt="%H:%M:%S", - level=logging.INFO, -) +bitrate = 1e6 # 1Mbps -parser = argparse.ArgumentParser( - prog="VulCAN Tester", - description="Send or Receive messages via CAN using a VulCAN adapter", -) +# Configure the connection to the VulCAN +bus = can.interface.Bus(channel="can0", interface="socketcan", bitrate=bitrate) -_ = parser.add_argument("--device", "-d", required=True, help="Path to VulCAN") -group = parser.add_mutually_exclusive_group(required=True) -_ = group.add_argument( - "--send", "-s", action="store_true", help="Send a basic CAN message" -) -_ = group.add_argument( - "--receive", "-r", action="store_true", help="Await a CAN message" -) -_ = group.add_argument( - "--pingpong", - "-p", - action="store_true", - help="Await a CAN message, then echo it back to the bus", -) +def drogue_should_acknowledge_deployment_msg(): + """ + Send a message to the drogue parachute board telling it to deploy. + Then, receive a message every half a second for two seconds (to avoid superfluous messages), + and check if it's the drogue board acknowledging deployment. If not, fail the test. + """ + deploy_drogue_msg = can.Message( + arbitration_id=0x100, data=[1], is_extended_id=False + ) + bus.send(deploy_drogue_msg) + acknowledged = 0 + time_start = time.time() -args = parser.parse_args() + while time.time() - time_start < 2.0: + msg = bus.recv(timeout=0.5) + if msg is not None and msg.arbitration_id == 0x101: + acknowledged = 1 -# Ensure proper arg types -device = cast(str, args.device) -device = str(device) -send = cast(bool, args.send) -send = bool(send) -receive = cast(bool, args.receive) -receive = bool(receive) -pingpong = cast(bool, args.pingpong) -pingpong = bool(pingpong) + assert acknowledged > 0 -def main(): - logger.info("Connecting to VulCAN") - bitrate = 1e6 +def main_should_acknowledge_deployment_msg(): + """ + Send a message to the main parachute board telling it to deploy. + Then, receive a message every half a second for two seconds (to avoid superfluous messages), + and check if it's the main board acknowledging deployment. If not, fail the test. + """ + deploy_main_msg = can.Message(arbitration_id=0x200, data=[1], is_extended_id=False) + bus.send(deploy_main_msg) + acknowledged = 0 + time_start = time.time() - # Configure the connection to the VulCAN - bus = can.interface.Bus(channel="can0", interface="socketcan", bitrate=bitrate) + while time.time() - time_start < 2.0: + msg = bus.recv(timeout=0.5) + if msg is not None and msg.arbitration_id == 0x201: + acknowledged = 1 - if send: - logger.info("Sending CAN message") - # Define a CAN message with an arbitrary ID and data bytes - message_id = 0x101 - data_bytes = [0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88] - message = can.Message( - arbitration_id=message_id, data=data_bytes, is_extended_id=False - ) - bus.send(message) - logger.info("Sent message: %s", message) - message_id = 0x201 - data_bytes = [0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88] - message = can.Message( - arbitration_id=message_id, data=data_bytes, is_extended_id=False - ) - bus.send(message) - logger.info("Sent message: %s", message) + assert acknowledged > 0 - if receive: - logger.info("Listening for CAN messages") - while True: - message = bus.recv(timeout=None) - logger.info("Received message: %s", message) - if pingpong: - logger.info("Echoing CAN messages") - while True: - message = bus.recv(timeout=None) - if message: - bus.send(message) - logger.info("Echoed message: %s", message) - - -if __name__ == "__main__": - main() +drogue_should_acknowledge_deployment_msg() +main_should_acknowledge_deployment_msg() diff --git a/controlSystem/RecoveryBoard/firmware-rs/tests/README.md b/controlSystem/RecoveryBoard/firmware-rs/tests/README.md new file mode 100644 index 0000000..4d63f0c --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/tests/README.md @@ -0,0 +1,30 @@ +## Testing + +Testing can be done in two different ways: + +1. [pytest](https://docs.pytest.org/en/stable/) +2. [embedded_test](https://docs.rs/embedded-test/0.7.0/embedded_test/) + +### Pytest + +Pytest is a testing framework written in Python. It is useful for this project to use it with a VulCAN adapter to test the CAN system of the ERS. + +### Embedded Test + +embedded_test is a testing framework written by the people at probe-rs. It allows us to write tests that get ran on the boards themselves. To create a new test, it is recommended that you copy the `blinky.rs` file as it contains the necessary boilerplate code to get the tests running, and rename it to `.rs` where `` is the name of the test you are writing. Then, in the `Config.toml` file at the root of the directory, add this block of code: + +```rust +[[test]] +name = "" +harness = false +``` + +It's important that you make sure all names are consistent, because `cargo test` will look for your test based on the name of the file and the name of the test in `Cargo.toml`. + +To write a test, follow the structure of the `blinky` test. It must have the `#[test]` procedural macro at the top. If it uses async functionalities like `.await`, it must be an async function. The `init()` function will be called before any `#[test]` and it will initialize the stm32. It will pass in to any `#[test]` function the struct containing all the peripherals, so if you add `p: Peripherals` to your function signature, you will gain access to the peripherals through the `p` variable. More detailed information on the `embedded_test` crate is available through the link at the top of this README. + +Then after you've written the test, run: + +```bash +cargo test -r --test +``` diff --git a/controlSystem/RecoveryBoard/firmware-rs/tests/blinky.rs b/controlSystem/RecoveryBoard/firmware-rs/tests/blinky.rs new file mode 100644 index 0000000..bc5c9d5 --- /dev/null +++ b/controlSystem/RecoveryBoard/firmware-rs/tests/blinky.rs @@ -0,0 +1,39 @@ +#![no_std] +#![no_main] + +#[cfg(test)] +#[embedded_test::tests] +mod tests { + use defmt::info; + use defmt_rtt as _; + use embassy_stm32::{ + gpio::{Level, Output, Speed}, + Peripherals, + }; + use embassy_time::Timer; + + #[init] + async fn init() -> Peripherals { + // Pass in the peripherals to each test function + embassy_stm32::init(Default::default()) + } + + #[test] + async fn blinky(p: Peripherals) { + info!("Testing 123"); + + let mut led = Output::new(p.PA5, Level::High, Speed::Low); + + #[cfg(main)] + let mut led = Output::new(p.PB14, Level::High, Speed::Low); + + for i in 1..=5 { + led.set_high(); + Timer::after_millis(250).await; + led.set_low(); + Timer::after_millis(250).await; + } + + assert!(true) + } +}