commit a688cd59157b8b4fd22b2a251aa6dac61e6a0dd2
Author: Daniel Pérez <steew@psi.my.domain>
Date: Sun Feb 01 22:16:44 2026 +0000
fctrl: implement CAN baseline communication, control loop start
diff --git a/fw/RustFM/Cargo.lock b/fw/RustFM/Cargo.lock
index 6a21320..76b5248 100644
--- a/fw/RustFM/Cargo.lock
+++ b/fw/RustFM/Cargo.lock
@@ -6869 +6869 @@ dependencies = [
[[package]]
name = "quote"
-version = "1.0.41"
+version = "1.0.40"
source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ce25767e7b499d1b604768e7cde645d14cc8584231ea6b295e9c9eb22c02e1d1"
+checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
dependencies = [
"proc-macro2",
]
diff --git a/fw/RustFM/src/comm.rs b/fw/RustFM/src/comm.rs
new file mode 100644
index 0000000..feaa966
--- /dev/null
+++ b/fw/RustFM/src/comm.rs
@@ -00 +134 @@
+use defmt::{trace, error};
+
+use embassy_stm32::can::filter::Mask32;
+use embassy_stm32::can::{
+ Fifo, Frame,
+};
+
+use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_sync::channel::{Channel, Receiver};
+use embassy_time::Timer;
+
+pub const CAN_BUFFER: usize = 10;
+
+#[embassy_executor::task]
+pub async fn comm_task(mut can_bus: embassy_stm32::can::Can<'static>, chan: Receiver<'static, ThreadModeRawMutex, u32, CAN_BUFFER>) {
+ // enable the fifo 0 bank for storing the CAN message queue
+ can_bus.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all());
+ // enable loopback for testing
+ can_bus.modify_config()
+ .set_loopback(true) // Receive own frames
+ .set_silent(true)
+ .set_bitrate(250_000);
+ can_bus.enable().await;
+
+ loop {
+ // wait for new messages to transmit
+ let message = chan.receive().await;
+ // TODO: check what to send in the message ID
+ let frame = Frame::new_extended(0x123456F, &message.to_le_bytes()).unwrap();
+ trace!("Writing CAN frame");
+
+ _ = can_bus.write(&frame).await;
+ }
+}
diff --git a/fw/RustFM/src/control.rs b/fw/RustFM/src/control.rs
new file mode 100644
index 0000000..10e039a
--- /dev/null
+++ b/fw/RustFM/src/control.rs
@@ -00 +166 @@
+use defmt::{info, trace};
+use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, channel::Receiver};
+use mpu6050_dmp::{quaternion::Quaternion, yaw_pitch_roll::YawPitchRoll};
+
+use crate::mpu::BUFFERED_QUATERNIONS;
+
+// represents the motor's possible turning direction (clockwise and counter-clockwise)
+pub enum MotorTurningState {
+ CW,
+ CCW
+}
+
+// holds the motor state, including its angular speed and direction.
+pub struct Motor {
+ angular_rotation_speed: f64,
+ turning: MotorTurningState,
+}
+
+// represents the body's total motor count
+pub const MOTOR_NUM: usize = 4;
+
+// holds the system state for control, including the motor rotation speeds, the current velocity and altitude,
+// and several other data relevant to system control
+pub struct Controller {
+ motors: [Motor; MOTOR_NUM],
+ altitude: f64,
+ velocity: f64,
+ last_ypr: YawPitchRoll,
+ // store the last and next quaternion values for the control calculus
+}
+
+#[embassy_executor::task]
+pub async fn update_control_loop(
+ quaternion_channel: Receiver<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>,
+) {
+ let mut last_yaw: f32 = 0.0;
+ let mut last_pitch: f32 = 0.0;
+ let mut last_roll: f32 = 0.0;
+
+ const THRESHOLD: f32 = 3.0;
+
+ loop {
+ let next_quaternion_value = quaternion_channel.receive().await;
+ let ypr_format = YawPitchRoll::from(next_quaternion_value);
+
+ let yaw_deg = ypr_format.yaw * 180.0 / core::f32::consts::PI;
+ let pitch_deg = ypr_format.pitch * 180.0 / core::f32::consts::PI;
+ let roll_deg = ypr_format.roll * 180.0 / core::f32::consts::PI;
+
+ let y_diff = yaw_deg - last_yaw;
+ let p_diff = pitch_deg - last_pitch;
+ let r_diff = roll_deg - last_roll;
+
+ // filter the new value and see if it has differed in more than THRESHOLD than the
+ // previous. If not, ignore it.
+ if f32::abs(y_diff) > THRESHOLD
+ || f32::abs(p_diff) > THRESHOLD
+ || f32::abs(r_diff) > THRESHOLD
+ {
+ info!("YPR differential: y:{}, p:{}, r:{}", y_diff, p_diff, r_diff);
+ last_yaw = yaw_deg;
+ last_pitch = pitch_deg;
+ last_roll = roll_deg;
+ }
+ }
+}
diff --git a/fw/RustFM/src/main.rs b/fw/RustFM/src/main.rs
index 072f861..deaad97 100644
--- a/fw/RustFM/src/main.rs
+++ b/fw/RustFM/src/main.rs
@@ -414 +419 @@
mod mpu;
mod rf;
mod status;
+mod control;
+mod comm;
use defmt::trace;
+use embassy_stm32::can;
+use embassy_stm32::can::Can;
+use embassy_stm32::peripherals::CAN1;
use embassy_stm32::time::Hertz;
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_sync::mutex::Mutex;
-use mpu::read_mpu;
use mpu::BUFFERED_QUATERNIONS;
+use comm::CAN_BUFFER;
use mpu6050_dmp::quaternion::Quaternion;
use rf::transmit;
use status::SystemStatus;
@@ -2018 +2518 @@ use embassy_executor::Spawner;
use embassy_stm32::{
bind_interrupts,
exti::ExtiInput,
- gpio::{AnyPin, Level, Output, Speed},
+ gpio::{Level, Output, Speed},
i2c::{self, I2c},
rcc::{self},
usart::{self, Uart},
- Config, Peri,
+ Config,
};
use embassy_sync::channel::Channel;
+use crate::{comm::comm_task, control::update_control_loop, mpu::read_mpu};
use crate::mpu::telemetry_sender;
use crate::status::status_leds;
use crate::status::SharedStatus;
-use embassy_time::Timer;
use {defmt_rtt as _, panic_probe as _};
@@ -396 +4410 @@ bind_interrupts!(struct Irqs {
I2C1_EV => i2c::EventInterruptHandler<embassy_stm32::peripherals::I2C1>;
I2C1_ER => i2c::ErrorInterruptHandler<embassy_stm32::peripherals::I2C1>;
USART3 => usart::InterruptHandler<embassy_stm32::peripherals::USART3>;
+ CAN1_RX0 => can::Rx0InterruptHandler<CAN1>;
+ CAN1_RX1 => can::Rx1InterruptHandler<CAN1>;
+ CAN1_SCE => can::SceInterruptHandler<CAN1>;
+ CAN1_TX => can::TxInterruptHandler<CAN1>;
});
#[embassy_executor::main]
@@ -626 +719 @@ async fn main(spawner: Spawner) {
});
let p = embassy_stm32::init(config);
+
+ let can_bus = Can::new(p.CAN1, p.PB8, p.PB9, Irqs);
+
let mut i2c_config = i2c::Config::default();
i2c_config.frequency = Hertz::khz(400);
i2c_config.gpio_speed = Speed::VeryHigh;
@@ -886 +1007 @@ async fn main(spawner: Spawner) {
static QUATERNION_CHANNEL: Channel<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS> =
Channel::<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>::new();
static SHARED_STATUS: SharedStatus = Mutex::new(SystemStatus::FAIL);
+ static CAN_MSG_QUEUE: Channel<ThreadModeRawMutex, u32, CAN_BUFFER> = Channel::<ThreadModeRawMutex, u32, CAN_BUFFER>::new();
let tmtry_uart = Uart::new(
p.USART3,
@@ -11014 +12319 @@ async fn main(spawner: Spawner) {
.spawn(read_mpu(iic, imu_int, QUATERNION_CHANNEL.sender()))
.unwrap();
// dedicated task for UART telemetry
+ #[cfg(telemetry)]
spawner
.spawn(telemetry_sender(tmtry_uart, QUATERNION_CHANNEL.receiver()))
.unwrap();
- // dedicated task for physical status leds
- // TODO: replace with timer interrupt
- // spawner.spawn(status_leds(ok_pin.into())).unwrap();
- // Timer::after_millis(500).await;
- // spawner.spawn(status_leds(fail_pin.into())).unwrap();
+ // dedicated task for CAN bus communications
+ spawner
+ .spawn(comm_task(can_bus, CAN_MSG_QUEUE.receiver()))
+ .unwrap();
+ // dedicated task for the quadcopter control loop
+ spawner
+ .spawn(update_control_loop(QUATERNION_CHANNEL.receiver()))
+ .unwrap();
+ // dedicated task for status leds
spawner
.spawn(status_leds(ok_pin.into(), fail_pin.into(), &SHARED_STATUS))
.unwrap();
diff --git a/fw/RustFM/src/mpu.rs b/fw/RustFM/src/mpu.rs
index 3e8163d..4bf6182 100644
--- a/fw/RustFM/src/mpu.rs
+++ b/fw/RustFM/src/mpu.rs
@@ -7511 +7511 @@ pub async fn read_mpu(
// .await
// .unwrap();
// trace!("Sensor Calibrated");
-
+ mpu.set_clock_source(mpu6050_dmp::clock_source::ClockSource::Xgyro).await.unwrap();
mpu.enable_dmp().await.unwrap();
mpu.load_firmware().await.unwrap();
mpu.boot_firmware().await.unwrap();
- mpu.set_sample_rate_divider(0).await.unwrap();
+ mpu.set_sample_rate_divider(4).await.unwrap();
mpu.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter0)
.await
.unwrap();
diff --git a/fw/RustFM/src/status.rs b/fw/RustFM/src/status.rs
index aba5c0c..2c5e88e 100644
--- a/fw/RustFM/src/status.rs
+++ b/fw/RustFM/src/status.rs
@@ -16 +15 @@
use embassy_stm32::{
- gpio::{AnyPin, Level, Output, Speed},
- Peri,
+ Peri, gpio::{AnyPin, Level, Output, Speed}, lptim::pwm::Pwm
};
use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, mutex::Mutex};
use embassy_time::Timer;
diff --git a/fw/live-plot/main.py b/fw/live-plot/main.py
index a796882..066a833 100644
--- a/fw/live-plot/main.py
+++ b/fw/live-plot/main.py
@@ -1024 +1028 @@ def main():
print("Opening serial port...")
ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
- figure, (y_plot, p_plot, r_plot) = plt.subplots(3, 1, figsize=(10, 6))
+ figure, (w_plot, x_plot, y_plot, z_plot) = plt.subplots(4, 1, figsize=(10, 6))
time_axis = np.arange(0, 10, 0.1)
+ w_data = np.zeros(time_axis.size)
+ x_data = np.zeros(time_axis.size)
y_data = np.zeros(time_axis.size)
- p_data = np.zeros(time_axis.size)
- r_data = np.zeros(time_axis.size)
+ z_data = np.zeros(time_axis.size)
# Starting plot
+ w_line, = w_plot.plot(time_axis, w_data, label='W Data')
+ x_line, = x_plot.plot(time_axis, x_data, label='X Data')
y_line, = y_plot.plot(time_axis, y_data, label='Y Data')
- p_line, = p_plot.plot(time_axis, p_data, label='P Data')
- r_line, = r_plot.plot(time_axis, r_data, label='R Data')
+ z_line, = z_plot.plot(time_axis, z_data, label='Z Data')
- y_plot.set(ylim=(-190, 190), yticks=np.arange(-190, 190, 50))
- p_plot.set(ylim=(-190, 190), yticks=np.arange(-190, 190, 50))
- r_plot.set(ylim=(-190, 190), yticks=np.arange(-190, 190, 50))
+ w_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50))
+ x_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50))
+ y_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50))
+ z_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50))
+ w_plot.set_title('W Data')
+ x_plot.set_title('X Data')
y_plot.set_title('Y Data')
- p_plot.set_title('P Data')
- r_plot.set_title('R Data')
+ z_plot.set_title('Z Data')
plt.tight_layout()
plt.ion() # Interactive mode on
@@ -3725 +4129 @@ def main():
loop = True
while loop:
try:
- read_bytes = ser.read(12)
+ read_bytes = ser.read(16)
- if len(read_bytes) == 12:
- p = struct.unpack('f', read_bytes[0:4])[0]
- y = struct.unpack('f', read_bytes[4:8])[0]
- r = struct.unpack('f', read_bytes[8:12])[0]
+ if len(read_bytes) == 16:
+ w = struct.unpack('f', read_bytes[0:4])[0]
+ x = struct.unpack('f', read_bytes[4:8])[0]
+ y = struct.unpack('f', read_bytes[8:12])[0]
+ z = struct.unpack('f', read_bytes[12:16])[0]
# shift the data and append the new values
+ w_data = np.roll(w_data, -1)
+ x_data = np.roll(x_data, -1)
y_data = np.roll(y_data, -1)
- p_data = np.roll(p_data, -1)
- r_data = np.roll(r_data, -1)
+ z_data = np.roll(z_data, -1)
- y_data[-1] = y
- p_data[-1] = p
- r_data[-1] = r
+ w_data[-1] = w
+ y_data[-1] = x
+ x_data[-1] = y
+ z_data[-1] = z
+ w_line.set_ydata(w_data)
+ x_line.set_ydata(x_data)
y_line.set_ydata(y_data)
- p_line.set_ydata(p_data)
- r_line.set_ydata(r_data)
+ z_line.set_ydata(z_data)
# Redraw the plots
plt.draw()
diff --git a/fw/octave/plot.m b/fw/octave/plot.m
index f57bda2..c7531ee 100644
--- a/fw/octave/plot.m
+++ b/fw/octave/plot.m
@@ -16 +18 @@
pkg load matgeom
pkg load instrument-control
+graphics_toolkit("qt")
+
tmtry_serial = serial("/dev/ttyUSB0", 9600);
srl_flush(tmtry_serial);
