From dda0a2304a6bbf8d4d511176cc027e7a13c4c02d Mon Sep 17 00:00:00 2001 From: Ryan Rodriguez Date: Sun, 8 Dec 2024 01:38:46 -0800 Subject: [PATCH] fix bug in control scaling, implement fast feedback loop, correct attitude handling --- src/liftoff_quad.rs | 367 +++++++++++++++++++++++++++----------------- 1 file changed, 226 insertions(+), 141 deletions(-) diff --git a/src/liftoff_quad.rs b/src/liftoff_quad.rs index a1284708..c11196ed 100644 --- a/src/liftoff_quad.rs +++ b/src/liftoff_quad.rs @@ -1,7 +1,7 @@ use binrw::{binrw, BinRead, BinWrite}; use cyber_rc::{cyberrc, Writer}; use nalgebra::{Matrix4, Quaternion, UnitQuaternion, Vector3}; -use peng_quad::config::LiftoffQuadrotorConfig; +use peng_quad::config::{LiftoffQuadrotorConfig, SimulationConfig}; use peng_quad::quadrotor::{QuadrotorInterface, QuadrotorState}; use peng_quad::SimulationError; use std::net::UdpSocket; @@ -21,9 +21,11 @@ pub struct LiftoffQuad { /// The last state of the quadrotor pub previous_state: QuadrotorState, /// Initial State - pub initial_state: QuadrotorState, + pub initial_state: Option, /// Simulation time step in seconds pub time_step: f32, + /// Config + pub simulation_config: SimulationConfig, /// Configured physical parameters pub config: LiftoffQuadrotorConfig, /// Previous Thrust @@ -31,15 +33,22 @@ pub struct LiftoffQuad { /// Previous Torque pub previous_torque: Vector3, /// Quadrotor sample mutex - pub consumer: watch::Receiver>, + pub producer: watch::Sender>>, + /// Quadrotor sample mutex + pub consumer: watch::Receiver>>, } impl LiftoffQuad { - pub fn new(time_step: f32, config: LiftoffQuadrotorConfig) -> Result { - let (producer, mut consumer) = watch::channel(None::); + pub fn new( + time_step: f32, + simulation_config: SimulationConfig, + config: LiftoffQuadrotorConfig, + ) -> Result { + let (producer, mut consumer) = watch::channel(None::>); let config_clone = config.clone(); + let producer_clone = producer.clone(); tokio::spawn(async move { - let _ = feedback_loop(config_clone, producer).await; + let _ = feedback_loop_fast(&config_clone.ip_address, producer_clone).await; }); // Open a serial port to communicate with the quadrotor if one is specified let writer: Option = match config.clone().serial_port { @@ -83,17 +92,18 @@ impl LiftoffQuad { None } }; - Ok(Self { writer, state: QuadrotorState::default(), previous_state: QuadrotorState::default(), - initial_state: QuadrotorState::default(), + initial_state: None, + simulation_config, config, time_step, previous_thrust: 0.0, previous_torque: Vector3::zeros(), consumer, + producer, }) } @@ -117,50 +127,67 @@ impl LiftoffQuad { // Combine all terms thrust_acceleration + gravity_body - rotational_acceleration } + + fn ensure_initial_is_set(&mut self, state: QuadrotorState) { + self.initial_state.get_or_insert(state); + } } impl QuadrotorInterface for LiftoffQuad { fn control( &mut self, - _: usize, + step_number: usize, thrust: f32, torque: &Vector3, ) -> Result<(), SimulationError> { - // Given thrust and torque, calculate the control inputs - // Clamp thrust and torque control inputs - let max_thrust = self.max_thrust(); - let thrust = thrust.clamp(0.0, max_thrust); - let max_torque = self.max_torque(); - let torque = Vector3::new( - torque.x.clamp(-max_torque.x, max_torque.x), - torque.y.clamp(-max_torque.y, max_torque.y), - torque.z.clamp(-max_torque.z, max_torque.z), - ); - - // Normalize inputs - let normalized_thrust = normalize(thrust, 0.0, self.max_thrust()); - let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); - let normalized_pitch = normalize(torque.y, -max_torque.y, max_torque.y); - let normalized_yaw = normalize(torque.z, -max_torque.z, max_torque.z); - - // Scale to RC commands - let throttle_command = scale_throttle(normalized_thrust); - let aileron_command = scale_control(normalized_roll); - let elevator_command = -scale_control(normalized_pitch); - let rudder_command = scale_control(normalized_yaw); - - let mut cyberrc_data = cyberrc::RcData { - throttle: throttle_command, - aileron: aileron_command, - elevator: elevator_command, - rudder: rudder_command, - arm: 0, - mode: 0, - }; - if let Some(writer) = &mut self.writer { - writer - .write(&mut cyberrc_data) - .map_err(|e| SimulationError::OtherError(e.to_string()))?; + if step_number + % (self.simulation_config.simulation_frequency + / self.simulation_config.control_frequency) + == 0 + { + // Given thrust and torque, calculate the control inputs + // Clamp thrust and torque control inputs + let max_thrust = self.max_thrust(); + // let thrust = thrust.clamp(0.0, 5.0); + let max_torque = self.max_torque(); + println!("Pitch Torque: {:?}", torque.y); + let torque = Vector3::new( + torque.x.clamp(-max_torque.x, max_torque.x), + torque.y.clamp(-10.0, 10.0), + // torque.z.clamp(-max_torque.z, max_torque.z), + torque.z.clamp(-6.0, 6.0), + ); + + // Normalize inputs + let normalized_thrust = normalize(thrust, 0.0, 5.0); + let normalized_roll = normalize(torque.x, -max_torque.x, max_torque.x); + let normalized_pitch = normalize(torque.y, -12.0, 12.0); + let normalized_yaw = normalize(torque.z, -6.0, 6.0); + println!( + "Thrust: {:?}, Normalized Thrust: {:?} Timestamp: {:?}", + thrust, normalized_thrust, self.state.time + ); + + // Scale to RC commands + let throttle_command = scale_throttle(normalized_thrust); + let aileron_command = scale_control(normalized_roll); + let elevator_command = -scale_control(normalized_pitch); + let rudder_command = scale_control(normalized_yaw); + + let mut cyberrc_data = cyberrc::RcData { + throttle: throttle_command, + // elevator: 0, // elevator_command, + aileron: aileron_command, + elevator: elevator_command, + rudder: rudder_command, + arm: 0, + mode: 0, + }; + if let Some(writer) = &mut self.writer { + writer + .write(&mut cyberrc_data) + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + } } Ok(()) } @@ -168,37 +195,78 @@ impl QuadrotorInterface for LiftoffQuad { /// Observe the current state of the quadrotor /// Returns a tuple containing the position, velocity, orientation, and angular velocity of the quadrotor. fn observe(&mut self) -> Result { - let sample = self.consumer.borrow_and_update().clone(); - - let state = match sample { - Some(sample) => { - // update the last state value - self.previous_state = self.state.clone(); - 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.previous_state.position) / self.time_step), - ); - - let omega_body = sample.pqr(); - let acceleration_body = - self.body_acceleration(attitude_quaternion, omega_body, v_body); - QuadrotorState { - position: sample_position, - velocity: v_body, - acceleration: acceleration_body, - orientation: attitude_quaternion, - angular_velocity: omega_body, - } + // TODO: if there is not a new packet, return the old state + if !self + .consumer + .has_changed() + .map_err(|e| SimulationError::OtherError(e.to_string()))? + { + return Ok(self.state.clone()); + } + self.previous_state = self.state.clone(); + let packet = self.consumer.borrow_and_update().clone(); + let sample = match packet { + Some(packet) => { + let mut cursor = std::io::Cursor::new(packet); + LiftoffPacket::read(&mut cursor) + .map_err(|e| SimulationError::OtherError(e.to_string())) } - None => self.state.clone(), + None => Err(SimulationError::OtherError("No packet".to_string())), + }?; + + let initial_state = self.initial_state.get_or_insert_with(|| { + QuadrotorState { + time: sample.timestamp, + position: sample.position(), + velocity: Vector3::zeros(), + acceleration: Vector3::zeros(), + // Store the initial yaw as the initial attitude so that we can adjust for it on + // subsequent samples + orientation: (|| -> UnitQuaternion { + let attitude = sample.attitude_quaternion(); + println!("Initial Yaw: {:?}", attitude.euler_angles().2.to_degrees()); + UnitQuaternion::new_normalize(Quaternion::new( + (1.0 - (attitude.k * attitude.k)).sqrt(), + 0.0, + 0.0, + -attitude.k, + )) + })(), + // orientation: sample.attitude_quaternion(), + angular_velocity: Vector3::zeros(), + } + }); + + // Adjust the sample by the initial state + // Adjust for the initial yaw - we take the starting yaw of the vehicle to be 0 + let attitude_quaternion = + initial_state.orientation.clone().conjugate() * sample.attitude_quaternion(); + // Asjut fot the initial position - we take the starting location of the vehicle to be the + // origin + let mut sample_position = sample.position(); + sample_position[0] -= initial_state.position[0]; + sample_position[1] -= initial_state.position[1]; + sample_position[2] -= initial_state.position[2]; + + // Calculate the body-frame velocity by rotating the inertial velocity using the + // attitude quaternion. + let v_body = attitude_quaternion.transform_vector( + &((sample_position - self.previous_state.position) + / (sample.timestamp - self.previous_state.time)), + ); + // sample_position[2] = -sample_position[2]; + + let omega_body = sample.pqr(); + let acceleration_body = self.body_acceleration(attitude_quaternion, omega_body, v_body); + self.state = QuadrotorState { + time: sample.timestamp as f32, + position: sample_position, + velocity: v_body, + acceleration: acceleration_body, + orientation: attitude_quaternion, + angular_velocity: omega_body, }; - self.state = state.clone(); - Ok(state) + Ok(self.state.clone()) } fn max_thrust(&self) -> f32 { @@ -209,7 +277,7 @@ impl QuadrotorInterface for LiftoffQuad { fn max_torque(&self) -> Vector3 { let motor_thrust = self.max_thrust() / 4.0; let max_rp_torque = 2.0 * 0.65 * motor_thrust; - let yaw_torque = 2.0 * 0.005 * motor_thrust; + let yaw_torque = 8.0 * motor_thrust; Vector3::new(max_rp_torque, max_rp_torque, yaw_torque) } @@ -246,83 +314,45 @@ impl LiftoffPacket { /// Returns the attitude quaternion in the NED frame pub fn attitude_quaternion(&self) -> UnitQuaternion { - // 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], - )); - // Flip the handedness by negating the Z component of the RUF quaternion. - // The orientation is now LUF - let flipped_quat = Quaternion::new(ruf_quat.w, ruf_quat.i, ruf_quat.j, -ruf_quat.k); - - // // 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 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); - - // Combine the handedness-adjusted quaternion with the rotation transformations - // Apply the Y rotation first, then the X rotation - UnitQuaternion::new_normalize(flipped_quat) + // Cast the attitude as a unit quaternion, and flip the handedness by inverting the z + // axis. + let r = UnitQuaternion::from_quaternion( + Quaternion::new( + self.attitude[3], + self.attitude[0], + self.attitude[1], + -self.attitude[2], + ) + .normalize(), + ); + let euler = r.euler_angles(); + UnitQuaternion::from_euler_angles(euler.2, euler.0, -euler.1) } - /// Translate Unity coordinates to NED coordinates + /// TODO: this is NEU + /// Translate Unity coordinates in RUF to NED coordinates pub fn position(&self) -> Vector3 { - Vector3::new(self.position[2], self.position[0], -self.position[1]) + Vector3::new(self.position[2], self.position[0], self.position[1]) } } -async fn feedback_loop( - liftoff_config: LiftoffQuadrotorConfig, - tx: watch::Sender>, +async fn feedback_loop_fast( + address: &str, + tx: watch::Sender>>, ) -> Result<(), SimulationError> { - let mut initial_position: Option = None; + let socket = tokio::net::UdpSocket::bind(address.to_string()) + .await + .map_err(|e| SimulationError::OtherError(e.to_string()))?; + let mut buf = [0; 2048]; loop { - let mut buf = [0; 128]; - // Possible bug: rebinding is necesssary to get new data from Liftoff. - let sample = match UdpSocket::bind(liftoff_config.ip_address.to_string()) { - Ok(socket) => { - socket - .set_read_timeout(Some(Duration::from_millis(50))) + match socket.recv_from(&mut buf).await { + Ok((len, _)) => { + let _ = tx + .send(Some(buf[..len].to_vec())) .map_err(|e| SimulationError::OtherError(e.to_string()))?; - match socket.recv_from(&mut buf) { - Ok((len, _)) => { - let mut cursor = std::io::Cursor::new(&buf[..len]); - // TODO: more robust handling of packet parsing errors during resets - let sample = if let Ok(mut sample) = LiftoffPacket::read(&mut cursor) { - if initial_position.is_none() { - initial_position = Some(sample.clone()); - } - // Position will be relative to initial, as Liftoff does not start the - // quad at 0, 0, 0 - let initial_position = initial_position.as_ref(); - sample.position[0] -= initial_position.unwrap().position[0]; - sample.position[1] -= initial_position.unwrap().position[1]; - sample.position[2] -= initial_position.unwrap().position[2]; - sample - } else { - LiftoffPacket::default() - }; - Some(sample) - } - Err(_) => None, - } - } - Err(e) => { - return Err(SimulationError::OtherError(format!( - "Bind loop exceeded max wait time {}", - e.to_string() - ))); } - }; - if sample.is_some() { - let _ = tx.send(sample).is_ok(); + Err(_) => (), } - // sleep(Duration::from_millis(10)).await; } } @@ -378,7 +408,7 @@ fn scale_throttle(thrust: f32) -> i32 { } fn scale_control(value: f32) -> i32 { - scale_to_rc_command_with_center(value, -32768 as f32, 32767 as f32, 0.0) + scale_to_rc_command_with_center(value, -32768 as f32, 0.0, 32737 as f32) } #[rustfmt::skip] @@ -388,3 +418,58 @@ const MOTOR_MIXING_MATRIX: Matrix4 = Matrix4::new( -1.0, -1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, ); + +#[cfg(test)] +mod tests { + + use super::*; + use std::f32::consts::FRAC_PI_2; + + /// Checks if two `Vector3` instances are element-wise close within the given tolerances. + fn is_close(this: &Vector3, other: &Vector3, rel_tol: f32, abs_tol: f32) -> bool { + fn is_close_scalar(a: f32, b: f32, rel_tol: f32, abs_tol: f32) -> bool { + (a - b).abs() <= (rel_tol * b.abs()).max(abs_tol) + } + + is_close_scalar(this.x, other.x, rel_tol, abs_tol) + && is_close_scalar(this.y, other.y, rel_tol, abs_tol) + && is_close_scalar(this.z, other.z, rel_tol, abs_tol) + } + + #[test] + fn test_acceleration_body() { + let quad = LiftoffQuad::new( + 0.01, + SimulationConfig::default(), + LiftoffQuadrotorConfig::default(), + ) + .unwrap(); + let omega = Vector3::zeros(); + let v = Vector3::zeros(); + + let q = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert_eq!(a, Vector3::new(0.0, 0.0, quad.config.gravity)); + + // Roll onto the right side, gravity should align with y direction to the "east" side of + // the drone + let q = UnitQuaternion::from_euler_angles(-FRAC_PI_2, 0.0, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert!(is_close( + &a, + &Vector3::new(0.0, quad.config.gravity, 0.0), + 1e-6, + 1e-6 + )); + + // Pitch forward 90 degrees, gravity should align with x direction down the nose + let q = UnitQuaternion::from_euler_angles(0.0, FRAC_PI_2, 0.0); + let a = quad.body_acceleration(q, omega, v); + assert!(is_close( + &a, + &Vector3::new(quad.config.gravity, 0.0, 0.0), + 1e-6, + 1e-6 + )); + } +}