diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index 133ad172..1e946409 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,25 +1,16 @@ use binrw::{binrw, BinRead}; use cyber_rc::{cyberrc, Writer}; -use nalgebra::{Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; -use peng_quad::config::{LiftoffConfiguration, QuadrotorConfig}; +use nalgebra::{AbstractRotation, Matrix4, Quaternion, Unit, UnitQuaternion, Vector3}; +use peng_quad::config::{self, LiftoffQuadrotorConfig, QuadrotorConfig, QuadrotorConfigurations}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use rand; use serialport::{available_ports, SerialPort, SerialPortBuilder, SerialPortType}; -use std::fmt::format; use std::net::UdpSocket; use std::sync::Arc; use tokio::sync::Mutex; use tokio::time::{sleep, Duration}; -#[rustfmt::skip] -const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( - 1.0, 1.0, 1.0, 1.0, - -1.0, 1.0, -1.0, 1.0, - -1.0, -1.0, 1.0, 1.0, - 1.0, -1.0, -1.0, 1.0, -); - /// Represents a quadrotor in the game Liftoff /// # Example /// ``` @@ -35,7 +26,7 @@ pub struct LiftoffQuad { /// Simulation time step in seconds pub time_step: f32, /// Configured physical parameters - pub config: QuadrotorConfig, + pub config: LiftoffQuadrotorConfig, /// Previous Thrust pub previous_thrust: f32, /// Previous Torque @@ -48,22 +39,17 @@ impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, _: usize, - config: &peng_quad::config::Config, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { // Given thrust and torque, calculate the control inputs - let roll_torque = torque[0]; - let pitch_torque = torque[1]; - let yaw_torque = torque[2]; - - let max_torques = config.quadrotor.max_torques(); + let max_torques = self.config.max_torques(); // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, config.quadrotor.max_thrust_kg); - let normalized_roll = normalize(roll_torque, -max_torques.0, max_torques.0); - let normalized_pitch = normalize(pitch_torque, -max_torques.1, max_torques.1); - let normalized_yaw = normalize(yaw_torque, -max_torques.2, max_torques.2); + let normalized_thrust = normalize(thrust, 0.0, self.config.max_thrust_kg); + let normalized_roll = normalize(torque[0], -max_torques.0, max_torques.0); + let normalized_pitch = normalize(torque[2], -max_torques.1, max_torques.1); + let normalized_yaw = normalize(torque[3], -max_torques.2, max_torques.2); // Scale to RC commands let throttle_command = scale_throttle(normalized_thrust); @@ -92,53 +78,75 @@ impl QuadrotorInterface for LiftoffQuad { while let Some(sample) = data_lock.take() { // update the last state value self.last_state = self.state.clone(); - // Store the sample values into temp variables - // Convert the position from Unity coordinates to NED coordinates - let position = vector3_ruf_to_ned(Vector3::from_row_slice(&sample.position)); - let velocity = (position - self.last_state.position) / self.time_step; - // Convert the orientation from Unity RUF to NED coordinates - let orientation = quaternion_ruf_to_ned(sample.attitude_quaternion()); - let delta_q = self.last_state.orientation * orientation.conjugate(); - let angle = 2.0 * delta_q.scalar().acos(); - let axis = delta_q - .axis() - .unwrap_or(Unit::new_normalize(Vector3::::zeros())); - let angular_velocity = axis.scale(angle / self.time_step); + let attitude_quaternion = sample.attitude_quaternion(); + // The position is in Unity inertial coordinates, so we transform to NED inertial + // coordinates. + let sample_position = sample.position(); + // Calculate the body-frame velocity by rotating the inertial velocity using the + // attitude quaternion. + let v_body = attitude_quaternion + .transform_vector(&((sample_position - self.last_state.position) / self.time_step)); + + let omega_body = sample.pqr(); + let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); self.state = QuadrotorState { - position, - velocity, - orientation, - angular_velocity, + position: sample_position, + velocity: v_body, + acceleration: acceleration_body, + orientation: attitude_quaternion, + angular_velocity: omega_body, }; } Ok(self.state.clone()) } + + fn read_imu(&self) -> Result<(Vector3, Vector3), SimulationError> { + todo!("") + } } impl LiftoffQuad { - fn new( - time_step: f32, - vehicle_config: QuadrotorConfig, - liftoff_config: LiftoffConfiguration, - ) -> Result { + pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { let shared_data: Arc>> = Arc::new(Mutex::new(None)); let shared_data_clone = Arc::clone(&shared_data); - tokio::spawn(async move { - let _ = feedback_loop(&liftoff_config, shared_data_clone).await; - }); + let config_clone = config.clone(); + // tokio::spawn(async move { + // let _ = feedback_loop(config_clone, shared_data_clone).await; + // }); let writer = Writer::new("COM3".to_string(), 115200) .map_err(|e| SimulationError::OtherError(e.to_string()))?; Ok(Self { writer: writer, state: QuadrotorState::default(), last_state: QuadrotorState::default(), - config: vehicle_config.clone(), + config, time_step, previous_thrust: 0.0, previous_torque: Vector3::zeros(), shared_data: Arc::new(Mutex::new(None)), }) } + + // Function to calculate body-frame acceleration for NED coordinates + fn body_acceleration( + &self, + q_inertial_to_body: UnitQuaternion, // Unit quaternion rotation from inertial to body + omega_body: Vector3, // Angular velocity in body frame + velocity_body: Vector3, // Linear velocity in body frame + ) -> Vector3 { + // Step 1: Calculate thrust acceleration (negative Z in NED body frame) + let thrust_acceleration = Vector3::new(0.0, 0.0, -self.previous_thrust / self.config.mass); + + // Step 2: Rotate the gravity vector to the body frame using the quaternion `apply` + let gravity_inertial = Vector3::new(0.0, 0.0, self.config.gravity); // NED gravity vector in inertial frame + let gravity_body = q_inertial_to_body.transform_vector(&gravity_inertial); // Rotate gravity vector to body frame + + // Step 3: Calculate rotational (Coriolis) effects + let rotational_acceleration = omega_body.cross(&velocity_body); + + // Step 4: Combine all terms + thrust_acceleration + gravity_body - rotational_acceleration + } } // TODO: configure packet based on the content of the Liftoff config file @@ -147,49 +155,57 @@ impl LiftoffQuad { #[derive(Debug)] pub struct LiftoffPacket { timestamp: f32, + // x, y, z position: [f32; 3], - // TODO: binrw read to quaternion + // x, y, z, w attitude: [f32; 4], + // pitch, roll, yaw - q, p, r + gyro: [f32; 3], motor_num: u8, #[br(count = motor_num)] motor_rpm: Vec, } impl LiftoffPacket { + /// Returns the angular velocity p, q, r + /// These are the roll rate, pitch rate, and yaw rate respectively + pub fn pqr(&self) -> Vector3 { + Vector3::new(self.gyro[1], self.gyro[0], self.gyro[2]) + } + + /// Returns the attitude quaternion in the NED frame pub fn attitude_quaternion(&self) -> UnitQuaternion { - UnitQuaternion::from_quaternion(Quaternion::new( + // The quaternion is in the Unity right-up-forward (RUF) frame + let ruf_quat = UnitQuaternion::from_quaternion(Quaternion::new( + self.attitude[3], self.attitude[0], self.attitude[1], self.attitude[2], - self.attitude[3], - )) - } -} - -fn quaternion_ruf_to_ned(ruf_quat: UnitQuaternion) -> UnitQuaternion { - // Step 1: Flip the handedness by negating the Z component of the RUF quaternion. - let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); + )); + // Flip the handedness by negating the Z component of the RUF quaternion. + let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); - // Step 2: Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) - let rotation_y = - UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); + // Define a 90-degree rotation around the Y-axis to align X (Right) to X (North) + let rotation_y = + UnitQuaternion::from_axis_angle(&Vector3::y_axis(), std::f32::consts::FRAC_PI_2); - // Step 3: Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) - let rotation_x = - UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); + // Define a -90-degree rotation around the X-axis to align Z (Forward) to Z (Down) + let rotation_x = + UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -std::f32::consts::FRAC_PI_2); - // Step 4: Combine the handedness-adjusted quaternion with the rotation transformations - // Apply the Y rotation first, then the X rotation - rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) -} + // Combine the handedness-adjusted quaternion with the rotation transformations + // Apply the Y rotation first, then the X rotation + rotation_x * rotation_y * UnitQuaternion::new_normalize(flipped_quat) + } -/// Translate Unity coordinates to NED coordinates -pub fn vector3_ruf_to_ned(unity_position: Vector3) -> Vector3 { - Vector3::new(unity_position[2], unity_position[0], -unity_position[1]) + /// Translate Unity coordinates to NED coordinates + pub fn position(&self) -> Vector3 { + Vector3::new(self.position[2], self.position[0], -self.position[1]) + } } async fn feedback_loop( - liftoff_config: &LiftoffConfiguration, + liftoff_config: LiftoffQuadrotorConfig, data_lock: Arc>>, ) -> Result<(), SimulationError> { let mut current_wait = Duration::from_secs(0); @@ -241,19 +257,55 @@ async fn feedback_loop( } } +/// Scale a value from a given range to a new range +/// # Example +/// ``` +/// let value = 0.0; +/// let min = -10.0; +/// let max = 10.0; +/// let scaled_value = normalize(value, min, max); +/// assert_eq!(scaled_value, 0.5); +/// ``` fn normalize(value: f32, min: f32, max: f32) -> f32 { (value - min) / (max - min) } -fn scale_to_rc_command(value: f32, min_output: f32, max_output: f32, center: f32) -> i32 { - let scaled_value = value * (max_output - center) + center; - scaled_value.clamp(min_output, max_output) as i32 +// TODO clean up all the type casts +// TODO assert value is normalized +fn scale_to_rc_command(value: f32, min: i32, max: i32, center: i32) -> i32 { + // assert value is in the range 0 to 1 + // Scale normalized value to the range between min and max + let range = max - min; + let scaled_value = min as f32 + value * range as f32; + + // Calculate the offset from the center within the range + let center_offset = scaled_value - (min as f32 + (range as f32) / 2.0); + + // Adjust the result around the center + (center as f32 + center_offset) as i32 + // scaled_value.clamp(max_output, min_output) as i32 } +/// Scale a thrust value to a throttle command +/// Throttle is inverted from Xinput to Liftoff +/// # Example +/// ``` +/// let thrust = 0.0; +/// let throttle = scale_throttle(thrust); +/// assert_eq!(throttle, 0); +/// ``` fn scale_throttle(thrust: f32) -> i32 { - scale_to_rc_command(thrust, 1000.0, 2000.0, 1500.0) + scale_to_rc_command(thrust, 32768, -32768, 32768) } fn scale_control(value: f32) -> i32 { - scale_to_rc_command(value, 1000.0, 2000.0, 1500.0) + scale_to_rc_command(value, -32768, 32767, 0) } + +#[rustfmt::skip] +const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( + 1.0, 1.0, 1.0, 1.0, + -1.0, 1.0, -1.0, 1.0, + -1.0, -1.0, 1.0, 1.0, + 1.0, -1.0, -1.0, 1.0, +);