diff --git a/common/src/command/set_pwm.rs b/common/src/command/set_pwm.rs index cc8d46d..6b305f5 100644 --- a/common/src/command/set_pwm.rs +++ b/common/src/command/set_pwm.rs @@ -3,7 +3,7 @@ use serde::{Deserialize, Serialize}; #[derive(Clone, Debug, Serialize, Deserialize)] pub struct SetPwm { - pub duty_cycle: u8 + pub throttle: f64 } impl Command for SetPwm {} diff --git a/flight/src/drive/mod.rs b/flight/src/drive/mod.rs new file mode 100644 index 0000000..3f3d70e --- /dev/null +++ b/flight/src/drive/mod.rs @@ -0,0 +1,24 @@ +use nautilus_common::command::set_pwm::SetPwm; +use nautilus_common::command::valid_priority_command::ValidPriorityCommand; +use std::time::Instant; + +pub mod task; + +pub trait Drive { + fn set_throttle(&self, throttle: f64, valid_until: Instant, priority: u8); + + + fn new_set_throttle_callback<'a>(&self) -> impl Fn(ValidPriorityCommand) + 'a + where + Self: Sized + Clone + 'a, + { + let this = self.clone(); + move |cmd| { + this.set_throttle( + cmd.throttle, + cmd.get_valid_until_instant(), + cmd.priority, + ); + } + } +} diff --git a/flight/src/drive/task.rs b/flight/src/drive/task.rs new file mode 100644 index 0000000..7361f75 --- /dev/null +++ b/flight/src/drive/task.rs @@ -0,0 +1,211 @@ +use std::any::type_name; +use std::fmt::{Debug, Formatter}; +use std::sync::mpsc::Receiver; +use std::time::Instant; +use log::{error, info, trace, warn}; +use crate::drive::Drive; +use crate::hardware::a4963::A4963; +use crate::hardware::pwm::PwmOutput; +use crate::scheduler::{CyclicTask, TaskHandle}; + +const OFF_THROTTLE: f64 = 0.0; +const THRESHOLD_THROTTLE: f64 = 0.2; +const MINIMUM_THROTTLE: f64 = 0.3; +const STARTUP_THROTTLE: f64 = 0.5; +const MAXIMUM_THROTTLE: f64 = 1.0; + +const STARTUP_TIME: u16 = 5; // 5 cycles = 0.5 second + +#[derive(Clone, Debug)] +pub enum DriveMessage { + SetThrust(f64), +} + +impl Drive for TaskHandle { + fn set_throttle(&self, throttle: f64, valid_until: Instant, priority: u8) { + trace!( + "TaskHandle::set_pin(self: {self:?}, throttle: {throttle}, valid_until: {valid_until:?}, priority: {priority})" + ); + // This can only fail if the other side is disconnected which we want to ignore + let _ = self.sender.send(DriveMessage::SetThrust(throttle)); + } +} + +pub struct DriveTask { + motor_controller: MotorController, + pwm: Pwm, + thrust_setpoint: f64, + state: State, +} + +impl Debug for DriveTask { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!( + f, + "DriveTask {{ thrust_setpoint: {}, state: {:?} }}", + type_name::(), + type_name::(), + self.thrust_setpoint, + self.state, + ) + } +} + +#[derive(Clone, Debug)] +enum State { + Off, + Configure, + Startup { + timer: u16, + }, + Operational, + Error, +} + +impl DriveTask +where + MotorController: A4963, + Pwm: PwmOutput +{ + pub fn new(motor_controller: MotorController, pwm: Pwm) -> Self { + Self { + motor_controller, + pwm, + thrust_setpoint: OFF_THROTTLE, + state: State::Off, + } + } + + fn step_state(&mut self, state: State) -> State { + trace!("DriveTask::step_state(self: {self:?}, state: {state:?})"); + match state { + State::Error => { + // A generic error state which cleans up + // any internals + self.thrust_setpoint = OFF_THROTTLE; + + State::Off + }, + State::Off => { + if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}"); + return state; + } + + if self.thrust_setpoint >= THRESHOLD_THROTTLE { + // Immediately start performing the startup + return self.step_state(State::Configure); + } + + state + } + State::Configure => { + let diagnostic = match self.motor_controller.check_health() { + Ok(diagnostic) => diagnostic, + Err(err) => { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}"); + return self.step_state(State::Error); + } + }; + if let Err(err) = diagnostic.assert_consistency() { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}") + } + if diagnostic.power_on_reset { + // We just had a power on or a reset event + // That's perfectly Ok as long as we only see it once + info!("Encountered a power on/reset"); + return state; + } + + match self.motor_controller.write_configuration() { + Ok(()) => State::Startup { + timer: 0, + }, + Err(err) => { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to write motor configuration: {err}"); + self.step_state(State::Error) + }, + } + } + State::Startup { timer } => { + if self.thrust_setpoint < THRESHOLD_THROTTLE { + return self.step_state(State::Off); + } + if let Err(err) = self.pwm.set_duty_cycle(STARTUP_THROTTLE) { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}"); + return self.step_state(State::Error); + } + + // remain in the current state + if timer < STARTUP_TIME { + State::Startup { + timer: timer.saturating_add(1) + } + } else { + State::Operational + } + } + State::Operational => { + if self.thrust_setpoint < THRESHOLD_THROTTLE { + return self.step_state(State::Off); + } + + let diagnostic = match self.motor_controller.check_health() { + Ok(diagnostic) => diagnostic, + Err(err) => { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}"); + return self.step_state(State::Error); + } + }; + if let Err(err) = diagnostic.assert_consistency() { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}") + } + if diagnostic.fault_flag { + if diagnostic.loss_of_synchronization { + if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}"); + return State::Error; + } + warn!("Loss of sync"); + return State::Startup { timer: 0 }; + } + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Diagnostic: {diagnostic:?}"); + return State::Error; + } + + let new_thrust = self.thrust_setpoint.clamp(MINIMUM_THROTTLE, MAXIMUM_THROTTLE); + if let Err(err) = self.pwm.set_duty_cycle(new_thrust) { + error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}"); + return self.step_state(State::Error); + } + + State::Operational + } + } + } +} + +impl CyclicTask for DriveTask +where + MotorController: A4963, + Pwm: PwmOutput +{ + type Message = DriveMessage; + type Data = (); + + fn get_data(&self) -> Self::Data { + () + } + + fn step(&mut self, receiver: &Receiver, step_time: Instant) { + trace!("DriveTask::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})"); + + while let Ok(message) = receiver.try_recv() { + match message { + DriveMessage::SetThrust(new_thrust) => self.thrust_setpoint = new_thrust, + } + } + + self.state = self.step_state(self.state.clone()); + } +} diff --git a/flight/src/hardware/a4963/driver.rs b/flight/src/hardware/a4963/driver.rs index 1d287cb..1c7d5d4 100644 --- a/flight/src/hardware/a4963/driver.rs +++ b/flight/src/hardware/a4963/driver.rs @@ -1,12 +1,10 @@ use std::any::type_name; use std::fmt::{Debug, Formatter}; -use std::thread::sleep; -use std::time::Duration; -use crate::hardware::a4963::A4963; +use crate::hardware::a4963::{A4963Diagnostic, A4963}; use anyhow::{ensure, Result}; use embedded_hal::spi::SpiDevice; -use log::{trace, warn}; -use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, DiagnosticRegister, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT}; +use log::trace; +use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT}; use crate::hardware::error::WrappingError; pub struct A4963Driver { @@ -25,24 +23,26 @@ where "A4963Driver::new(spi)", type_name::() ); - Self { + let mut result = Self { spi, mask_register: MaskRegister { ..Default::default() }, - } + }; + + let _ = result.write(RunRegister { + enable: false, + ..Default::default() + }); + + result } - fn update_mask_register(&mut self, mask_register: MaskRegister) -> Result { - self.mask_register = mask_register; - self.write_verify(self.mask_register.clone()) - } - - fn read_diagnostic_register(&mut self) -> Result { + fn read_diagnostic_register(&mut self) -> Result { self.write(self.mask_register.clone()) } - fn write(&mut self, register: Register) -> Result { + fn write(&mut self, register: Register) -> Result { trace!("A4963Driver::write(self: {self:?}, register: {register:?})"); let mut rx_buffer = [0u8; 2]; let tx_data = register.encode() | Register::ADDRESS | WRITE_BIT; @@ -51,7 +51,7 @@ where self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?; let rx_data = u16::from_be_bytes(rx_buffer); trace!("A4963Driver::write - rx = {rx_data:016b}"); - Ok(DiagnosticRegister::decode(rx_data)) + Ok(A4963Diagnostic::decode(rx_data)) } fn read(&mut self) -> Result { @@ -62,12 +62,12 @@ where self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?; let rx_data = u16::from_be_bytes(rx_buffer); trace!("A4963Driver::read - rx {} = {rx_data:016b}", type_name::()); - DiagnosticRegister::decode(rx_data & 0b1110_0000_0000_0000) + A4963Diagnostic::decode(rx_data & 0b1110_0000_0000_0000) .assert_healthy()?; Ok(Register::decode(rx_data)) } - fn write_verify(&mut self, register: Register) -> Result { + fn write_verify(&mut self, register: Register) -> Result { let result = self.write(register.clone())?; let readback = self.read()?; @@ -84,14 +84,15 @@ where SPI::Error: Sync, SPI::Error: 'static, { - fn init(&mut self) -> Result<()> { - trace!("A4963Driver::init(self: {self:?})"); + fn check_health(&mut self) -> Result { + trace!("A4963Driver::check_health(self: {self:?})"); + self.read_diagnostic_register() + } - // Write the fault mask first - self.update_mask_register(MaskRegister { - enable_loss_of_synchronization: false, - ..Default::default() - })?.assert_healthy()?; + fn write_configuration(&mut self) -> Result<()> { + trace!("A4963Driver::write_configuration(self: {self:?})"); + + self.check_health()?.assert_healthy()?; // Write our configurations self.write_verify(Configuration0Register { ..Default::default() @@ -105,33 +106,34 @@ where ..Default::default() })?.assert_healthy()?; self.write_verify(Configuration3Register { + position_integral_gain: 0b1001, ..Default::default() })?.assert_healthy()?; self.write_verify(Configuration4Register { - forced_startup_torque_duty_cycle: 0b0111, + forced_startup_torque_duty_cycle: 0b1111, start_speed: 0b1111, ..Default::default() })?.assert_healthy()?; self.write_verify(Configuration5Register { - speed_output_selection: SpeedOutputSelection::CommutationFrequency, + speed_integral_gain: 0b0111, + speed_output_selection: SpeedOutputSelection::ElectricalFrequency, // 819.1Hz = 49146 rpm max speed (divide by number of motor pole pairs) // Measured maximum of the motor is ~840Hz maximum_electrical_cycle_frequency: 0b101, + phase_advance: 0b0100, ..Default::default() })?.assert_healthy()?; // Set up the run mode for the motor (this enables the motor) self.write_verify(RunRegister { motor_control_mode: MotorControlMode::ClosedLoopSpeed, + restart_after_loss_of_sync: false, // I want to handle this in my code direction: Direction::Forward, // A = Black, B = Yellow, C = Red + enable: true, ..Default::default() })?.assert_healthy()?; Ok(()) } - - fn check_health(&mut self) -> Result<()> { - self.read_diagnostic_register()?.assert_healthy() - } } impl Debug for A4963Driver { diff --git a/flight/src/hardware/a4963/mod.rs b/flight/src/hardware/a4963/mod.rs index 054368e..dd23011 100644 --- a/flight/src/hardware/a4963/mod.rs +++ b/flight/src/hardware/a4963/mod.rs @@ -3,10 +3,30 @@ mod register; use anyhow::Result; -pub trait A4963 { - fn init(&mut self) -> Result<()>; +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct A4963Diagnostic { + pub fault_flag: bool, + pub power_on_reset: bool, + pub serial_transfer_error: bool, + pub high_temperature_warning: bool, + pub overtemperature_shutdown: bool, + pub loss_of_synchronization: bool, + pub undervoltage: bool, + pub phase_a_high_side_fault: bool, + pub phase_a_low_side_fault: bool, + pub phase_b_high_side_fault: bool, + pub phase_b_low_side_fault: bool, + pub phase_c_high_side_fault: bool, + pub phase_c_low_side_fault: bool, +} - fn check_health(&mut self) -> Result<()>; +pub trait A4963 { + + fn check_health(&mut self) -> Result; + + fn write_configuration(&mut self) -> Result<()>; + + // fn check_health(&mut self) -> Result<()>; } pub use driver::A4963Driver; diff --git a/flight/src/hardware/a4963/register.rs b/flight/src/hardware/a4963/register.rs index c8d81fd..6c1b1b7 100644 --- a/flight/src/hardware/a4963/register.rs +++ b/flight/src/hardware/a4963/register.rs @@ -1,8 +1,9 @@ use std::fmt::Debug; use std::hint::unreachable_unchecked; use strum_macros::FromRepr; -use anyhow::Result; +use anyhow::{ensure, Result}; use thiserror::Error; +use crate::hardware::a4963::A4963Diagnostic; pub(super) const WRITE_BIT: u16 = 0b0001_0000_0000_0000; @@ -485,23 +486,6 @@ impl A4963Register for RunRegister { } } -#[derive(Clone, Debug, PartialEq, Eq)] -pub(super) struct DiagnosticRegister { - pub fault_flag: bool, - pub power_on_reset: bool, - pub serial_transfer_error: bool, - pub high_temperature_warning: bool, - pub overtemperature_shutdown: bool, - pub loss_of_synchronization: bool, - pub undervoltage: bool, - pub phase_a_high_side_fault: bool, - pub phase_a_low_side_fault: bool, - pub phase_b_high_side_fault: bool, - pub phase_b_low_side_fault: bool, - pub phase_c_high_side_fault: bool, - pub phase_c_low_side_fault: bool, -} - #[derive(Clone, Debug, Error, PartialEq, Eq)] enum DiagnosticRegisterError { #[error("Unknown Fault")] @@ -532,7 +516,7 @@ enum DiagnosticRegisterError { PhaseCLowSide, } -impl DiagnosticRegister { +impl A4963Diagnostic { pub(super) const fn decode(value: u16) -> Self { Self { fault_flag: (value & 0b1000_0000_0000_0000) != 0, @@ -552,6 +536,25 @@ impl DiagnosticRegister { } } + pub fn assert_consistency(&self) -> Result<()> { + let measured_fault = self.power_on_reset || + self.serial_transfer_error || + self.high_temperature_warning || + self.overtemperature_shutdown || + self.loss_of_synchronization || + self.undervoltage || + self.phase_a_high_side_fault || + self.phase_a_low_side_fault || + self.phase_b_high_side_fault || + self.phase_b_low_side_fault || + self.phase_c_high_side_fault || + self.phase_c_low_side_fault; + + ensure!(self.fault_flag == measured_fault, "Measured Diagnostic {self:?} is Inconsistent with Fault Flag"); + + Ok(()) + } + pub fn assert_healthy(self) -> Result<()> { if self.power_on_reset { Err(DiagnosticRegisterError::PowerOnReset.into()) diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index a26e906..b786da0 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -1,15 +1,15 @@ use crate::hardware::mcp23017::Mcp23017; use anyhow::Result; -use embedded_hal::pwm::SetDutyCycle; use log::trace; use std::fmt::Debug; use crate::hardware::a4963::A4963; +use crate::hardware::pwm::PwmOutput; pub trait Hardware { type Mcp23017<'a>: Mcp23017 + Send + Debug where Self: 'a; - type Pwm: SetDutyCycle + Sync + Send; + type Pwm: PwmOutput + Sync + Send; fn new_mcp23017_a(&self) -> Result>; fn new_mcp23017_b(&self) -> Result>; @@ -52,3 +52,4 @@ mod mcp3208; pub mod pin; mod sim; pub mod a4963; +pub mod pwm; diff --git a/flight/src/hardware/pwm.rs b/flight/src/hardware/pwm.rs new file mode 100644 index 0000000..40900da --- /dev/null +++ b/flight/src/hardware/pwm.rs @@ -0,0 +1,6 @@ + +use anyhow::Result; + +pub trait PwmOutput { + fn set_duty_cycle(&mut self, duty_cycle: f64) -> Result<()>; +} diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index 99be530..302f49f 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -13,7 +13,6 @@ use rpi_pal::spi::SimpleHalSpiDevice; use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi}; use std::cell::RefCell; use std::sync::Mutex; -use rpi_pal::gpio::{Gpio, InputPin}; use crate::hardware::a4963::{A4963Driver, A4963}; use crate::hardware::channelization::PininChannel; diff --git a/flight/src/hardware/raspi/pwm.rs b/flight/src/hardware/raspi/pwm.rs index 1a3f703..7c7c72b 100644 --- a/flight/src/hardware/raspi/pwm.rs +++ b/flight/src/hardware/raspi/pwm.rs @@ -1,8 +1,8 @@ -use crate::hardware::error::WrappingError; -use embedded_hal::pwm::{ErrorType, SetDutyCycle}; use log::trace; use rpi_pal::pwm::Pwm; use std::time::Duration; +use crate::hardware::pwm::PwmOutput; +use anyhow::Result; const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz @@ -12,29 +12,20 @@ pub struct PwmWrapper { } impl PwmWrapper { - pub fn new(mut pwm: Pwm) -> anyhow::Result { + pub fn new(mut pwm: Pwm) -> Result { trace!("PwmWrapper::new(pwm: {pwm:?})"); pwm.set_period(PWM_PERIOD)?; + pwm.set_duty_cycle(0.0)?; pwm.enable()?; pwm.set_reset_on_drop(true); Ok(Self { pwm }) } } -impl ErrorType for PwmWrapper { - type Error = WrappingError; -} +impl PwmOutput for PwmWrapper { -impl SetDutyCycle for PwmWrapper { - fn max_duty_cycle(&self) -> u16 { - trace!("PwmWrapper::max_duty_cycle(self: {self:?})"); - u16::MAX - } - - fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { - trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})"); - self.pwm - .set_duty_cycle((duty as f64) / (u16::MAX as f64)) - .map_err(WrappingError) + fn set_duty_cycle(&mut self, duty_cycle: f64) -> Result<()> { + trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty_cycle: {duty_cycle})"); + Ok(self.pwm.set_duty_cycle(duty_cycle)?) } } diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 43a6f3f..3769494 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,6 +1,5 @@ #![warn(clippy::all, clippy::pedantic)] -use std::f64::consts::{PI, TAU}; use crate::comms::{CommsState, CommsTask}; use crate::hardware::Hardware; use crate::hardware::initialize; @@ -10,8 +9,7 @@ use crate::rcs::RcsTask; use crate::scheduler::Scheduler; use crate::state_vector::StateVector; use anyhow::Result; -use embedded_hal::pwm::{ErrorType, SetDutyCycle}; -use log::{error, info}; +use log::info; use nautilus_common::add_ctrlc_handler_arc; use nautilus_common::telemetry::{SwitchBank, TelemetryMessage}; use std::sync::Arc; @@ -19,11 +17,21 @@ use std::sync::atomic::{AtomicBool, Ordering}; use std::thread::sleep; use std::time::Duration; use crc::Algorithm; -use nautilus_common::command::set_pwm::SetPwm; -use crate::hardware::a4963::A4963; +use crate::drive::Drive; +use crate::drive::task::DriveTask; mod hardware; +mod commanded_state; +mod comms; +mod data; +mod rcs; +mod scheduler; +mod state_vector; +mod drive; +#[cfg(test)] +mod test_utils; + pub const CRC_8_CCITT: Algorithm = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF }; fn new_shutdown_handler(running: &Arc) -> impl Fn(()) { @@ -48,20 +56,15 @@ pub fn run() -> Result<()> { let mut hal = initialize()?; - let mut a4963 = hal.new_a4963()?; + let a4963 = hal.new_a4963()?; let mut mcp23017_a = hal.new_mcp23017_a()?; let mut mcp23017_b = hal.new_mcp23017_b()?; - let mut pwm0 = hal.new_pwm0()?; + let pwm0 = hal.new_pwm0()?; info!("Battery Voltage: {}", hal.get_battery_voltage()?); - pwm0.set_duty_cycle_percent(0)?; - mcp23017_a.init()?; mcp23017_b.init()?; - a4963.init()?; - - pwm0.set_duty_cycle_percent(100)?; Scheduler::scope(running.clone(), |s| { let task_a = s.run_cyclic( @@ -80,19 +83,18 @@ pub fn run() -> Result<()> { let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?; + let drive = s.run_cyclic( + "drive-task", + DriveTask::new(a4963, pwm0), + 10 + )?; + let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?; comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?; comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?; comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?; comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?; - comms.add_command_handler("/pwm/set", move |pwm: SetPwm| { - match pwm0.set_duty_cycle_percent(pwm.duty_cycle) { - Ok(_) => {} - Err(err) => { - error!("Failed to set PWM duty cycle: {err}"); - } - } - })?; + comms.add_command_handler("/drive/throttle/set", drive.new_set_throttle_callback())?; let comms = s.run_cyclic("comms-task", comms, 10)?; let comms_id = *comms; @@ -122,8 +124,7 @@ pub fn run() -> Result<()> { info!("Starting Main Loop"); while running.load(Ordering::Relaxed) { - sleep(Duration::from_millis(100)); - a4963.check_health()?; + sleep(Duration::from_millis(1000)); } anyhow::Ok(()) @@ -142,12 +143,3 @@ pub fn run() -> Result<()> { Ok(()) } - -mod commanded_state; -mod comms; -mod data; -mod rcs; -mod scheduler; -mod state_vector; -#[cfg(test)] -mod test_utils; diff --git a/ground/src/command.rs b/ground/src/command.rs index 432a829..b5564e5 100644 --- a/ground/src/command.rs +++ b/ground/src/command.rs @@ -23,6 +23,7 @@ use nautilus_common::command::set_pwm::SetPwm; const MAX_DATETIME: DateTime = DateTime::from_timestamp_nanos(i64::MAX); const PIN_PRIORITY: u8 = 1; const RCS_PRIORITY: u8 = 1; +const DRIVE_PRIORITY: u8 = 1; pub struct CommandHandler<'a> { @@ -66,12 +67,12 @@ impl From for SetRcs { #[derive(IntoCommandDefinition)] struct PwmCommand { - duty_cycle: u8 + throttle: f64 } impl From for SetPwm { fn from(value: PwmCommand) -> Self { Self { - duty_cycle: value.duty_cycle + throttle: value.throttle } } } @@ -158,10 +159,14 @@ impl<'a> CommandHandler<'a> { { let outgoing_commands_tx = outgoing_commands_tx.clone(); - commands.push(self.cmd.register_handler("pwm.set", move |_, cmd: PwmCommand| -> anyhow::Result<_> { + commands.push(self.cmd.register_handler("drive.throttle.set", move |header, cmd: PwmCommand| -> anyhow::Result<_> { trace!("Sending Pwm Set Command"); - outgoing_commands_tx.try_send_command("/pwm/set", &SetPwm::from(cmd))?; + outgoing_commands_tx.try_send_command("/drive/throttle/set", &ValidPriorityCommand { + inner: SetPwm::from(cmd), + valid_until: header.timestamp + TimeDelta::seconds(5), + priority: DRIVE_PRIORITY, + })?; Ok("Command Executed Successfully".to_string()) }));