diff --git a/fbw-a32nx/src/wasm/systems/a320_systems/src/hydraulic/mod.rs b/fbw-a32nx/src/wasm/systems/a320_systems/src/hydraulic/mod.rs index dbe24a34560..c376b00f794 100644 --- a/fbw-a32nx/src/wasm/systems/a320_systems/src/hydraulic/mod.rs +++ b/fbw-a32nx/src/wasm/systems/a320_systems/src/hydraulic/mod.rs @@ -1589,11 +1589,13 @@ impl A320Hydraulic { nose_steering: SteeringActuator::new( context, + "NOSE_WHEEL", Angle::new::(75.), AngularVelocity::new::(0.35), Length::new::(0.075), Ratio::new::(0.18), Pressure::new::(2000.), + true, ), core_hydraulic_updater: MaxStepLoop::new(Self::HYDRAULIC_SIM_TIME_STEP), @@ -3537,8 +3539,6 @@ struct A320HydraulicBrakeSteerComputerUnit { left_brake_pedal_input_id: VariableIdentifier, right_brake_pedal_input_id: VariableIdentifier, - ground_speed_id: VariableIdentifier, - rudder_pedal_input_id: VariableIdentifier, tiller_handle_input_id: VariableIdentifier, tiller_pedal_disconnect_id: VariableIdentifier, @@ -3567,8 +3567,6 @@ struct A320HydraulicBrakeSteerComputerUnit { tiller_steering_limiter: SteeringAngleLimiter<5>, tiller_input_map: SteeringRatioToAngle<6>, final_steering_position_request: Angle, - - ground_speed: Velocity, } impl A320HydraulicBrakeSteerComputerUnit { const RUDDER_PEDAL_INPUT_GAIN: f64 = 32.; @@ -3611,7 +3609,6 @@ impl A320HydraulicBrakeSteerComputerUnit { right_brake_pedal_input_id: context .get_identifier("RIGHT_BRAKE_PEDAL_INPUT".to_owned()), - ground_speed_id: context.get_identifier("GPS GROUND SPEED".to_owned()), rudder_pedal_input_id: context.get_identifier("RUDDER_PEDAL_POSITION_RATIO".to_owned()), tiller_handle_input_id: context.get_identifier("TILLER_HANDLE_POSITION".to_owned()), tiller_pedal_disconnect_id: context @@ -3656,8 +3653,6 @@ impl A320HydraulicBrakeSteerComputerUnit { Self::TILLER_INPUT_CURVE_MAP, ), final_steering_position_request: Angle::new::(0.), - - ground_speed: Velocity::new::(0.), } } @@ -3720,7 +3715,7 @@ impl A320HydraulicBrakeSteerComputerUnit { engine1: &impl Engine, engine2: &impl Engine, ) { - self.update_steering_demands(lgciu1, engine1, engine2); + self.update_steering_demands(context, lgciu1, engine1, engine2); self.update_normal_braking_availability(current_pressure.pressure()); self.update_brake_pressure_limitation(); @@ -3798,6 +3793,7 @@ impl A320HydraulicBrakeSteerComputerUnit { fn update_steering_demands( &mut self, + context: &UpdateContext, lgciu1: &impl LgciuInterface, engine1: &impl Engine, engine2: &impl Engine, @@ -3815,7 +3811,7 @@ impl A320HydraulicBrakeSteerComputerUnit { // TODO Here ground speed would be probably computed from wheel sensor logic let final_steer_rudder_plus_autopilot = self.pedal_steering_limiter.angle_from_speed( - self.ground_speed, + context.ground_speed(), (steer_angle_from_pedals + steer_angle_from_autopilot) .min(Angle::new::( Self::MAX_RUDDER_INPUT_INCLUDING_AUTOPILOT_DEGREE, @@ -3826,7 +3822,7 @@ impl A320HydraulicBrakeSteerComputerUnit { ); let steer_angle_from_tiller = self.tiller_steering_limiter.angle_from_speed( - self.ground_speed, + context.ground_speed(), self.tiller_input_map .angle_demand_from_input_demand(self.tiller_handle_position), ); @@ -3877,7 +3873,6 @@ impl SimulationElement for A320HydraulicBrakeSteerComputerUnit { Ratio::new::(reader.read(&self.tiller_handle_input_id)); self.rudder_pedal_position = Ratio::new::(reader.read(&self.rudder_pedal_input_id)); self.tiller_pedal_disconnect = reader.read(&self.tiller_pedal_disconnect_id); - self.ground_speed = reader.read(&self.ground_speed_id); self.autopilot_nosewheel_demand = Ratio::new::(reader.read(&self.autopilot_nosewheel_demand_id)); @@ -6004,7 +5999,7 @@ mod tests { use uom::si::{ angle::degree, - angle::radian, + angular_velocity::degree_per_second, electric_potential::volt, length::foot, mass_density::kilogram_per_cubic_meter, @@ -6886,8 +6881,9 @@ mod tests { self } - fn set_pushback_angle(mut self, angle: Angle) -> Self { - self.write_by_name("PUSHBACK ANGLE", angle.get::()); + fn set_pushback_angle(mut self, angle: AngularVelocity) -> Self { + self.write_by_name("ROTATION VELOCITY BODY Y", angle.get::()); + self.write_by_name("VELOCITY BODY Z", -1.); self } @@ -10439,14 +10435,14 @@ mod tests { .set_tiller_demand(Ratio::new::(1.)) .run_waiting_for(Duration::from_secs_f64(5.)); - assert!(test_bed.nose_steering_position().get::() >= 73.9); + assert!(test_bed.nose_steering_position().get::() >= 73.5); assert!(test_bed.nose_steering_position().get::() <= 75.1); test_bed = test_bed .set_tiller_demand(Ratio::new::(-1.)) .run_waiting_for(Duration::from_secs_f64(10.)); - assert!(test_bed.nose_steering_position().get::() <= -73.9); + assert!(test_bed.nose_steering_position().get::() <= -73.5); assert!(test_bed.nose_steering_position().get::() >= -75.1); } @@ -10999,16 +10995,8 @@ mod tests { test_bed = test_bed .set_pushback_state(true) - .set_pushback_angle(Angle::new::(80.)) - .run_waiting_for(Duration::from_secs_f64(0.5)); - - // Do not turn instantly in 0.5s - assert!( - test_bed.get_nose_steering_ratio() > Ratio::new::(0.) - && test_bed.get_nose_steering_ratio() < Ratio::new::(0.5) - ); - - test_bed = test_bed.run_waiting_for(Duration::from_secs_f64(5.)); + .set_pushback_angle(AngularVelocity::new::(-5.)) + .run_waiting_for(Duration::from_secs_f64(5.)); // Has turned fully after 5s assert!(test_bed.get_nose_steering_ratio() > Ratio::new::(0.9)); @@ -11016,12 +11004,8 @@ mod tests { // Going left test_bed = test_bed .set_pushback_state(true) - .set_pushback_angle(Angle::new::(-80.)) - .run_waiting_for(Duration::from_secs_f64(0.5)); - - assert!(test_bed.get_nose_steering_ratio() > Ratio::new::(0.2)); - - test_bed = test_bed.run_waiting_for(Duration::from_secs_f64(5.)); + .set_pushback_angle(AngularVelocity::new::(5.)) + .run_waiting_for(Duration::from_secs_f64(5.)); // Has turned fully left after 5s assert!(test_bed.get_nose_steering_ratio() < Ratio::new::(-0.9)); diff --git a/fbw-a380x/src/base/flybywire-aircraft-a380-842/SimObjects/AirPlanes/FlyByWire_A380_842/model/A380_EXTERIOR.xml b/fbw-a380x/src/base/flybywire-aircraft-a380-842/SimObjects/AirPlanes/FlyByWire_A380_842/model/A380_EXTERIOR.xml index 39d268b7a8a..8a815302f4d 100755 --- a/fbw-a380x/src/base/flybywire-aircraft-a380-842/SimObjects/AirPlanes/FlyByWire_A380_842/model/A380_EXTERIOR.xml +++ b/fbw-a380x/src/base/flybywire-aircraft-a380-842/SimObjects/AirPlanes/FlyByWire_A380_842/model/A380_EXTERIOR.xml @@ -116,14 +116,14 @@ left_bws left_bws - (L:A32NX_BODY_WHEEL_STEERING_POSITION) + (L:A32NX_LEFT_BODY_WHEEL_STEERING_POSITION) 1 right_bws right_bws - (L:A32NX_BODY_WHEEL_STEERING_POSITION) + (L:A32NX_RIGHT_BODY_WHEEL_STEERING_POSITION) 1 diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/fuel_quantity_management_system.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/fuel_quantity_management_system.rs index 4247161bb6d..b938eb22b25 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/fuel_quantity_management_system.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/fuel_quantity_management_system.rs @@ -66,12 +66,6 @@ pub struct RefuelPanelInput { refuel_rate_setting: RefuelRate, refuel_rate_setting_id: VariableIdentifier, - ground_speed_id: VariableIdentifier, - ground_speed: Velocity, - - is_on_ground_id: VariableIdentifier, - is_on_ground: bool, - engine_state_ids: [VariableIdentifier; 4], engine_states: [EngineState; 4], @@ -93,10 +87,6 @@ impl RefuelPanelInput { refuel_rate_setting_id: context.get_identifier("EFB_REFUEL_RATE_SETTING".to_owned()), refuel_rate_setting: RefuelRate::Real, - ground_speed_id: context.get_identifier("GPS GROUND SPEED".to_owned()), - ground_speed: Velocity::default(), - is_on_ground_id: context.get_identifier("SIM ON GROUND".to_owned()), - is_on_ground: false, engine_state_ids: [1, 2, 3, 4] .map(|id| context.get_identifier(format!("ENGINE_STATE:{id}"))), @@ -131,15 +121,15 @@ impl RefuelPanelInput { self.refuel_rate_setting } - fn refuel_is_enabled(&self) -> bool { + fn refuel_is_enabled(&self, context: &UpdateContext) -> bool { // TODO: Should this be if two engines are running instead of just one? self.refuel_status && self .engine_states .iter() .all(|state| *state == EngineState::Off) - && self.is_on_ground - && self.ground_speed < Velocity::new::(0.1) + && context.is_on_ground() + && context.ground_speed() < Velocity::new::(0.1) } fn target_zero_fuel_weight(&self) -> Mass { @@ -156,8 +146,7 @@ impl SimulationElement for RefuelPanelInput { Mass::new::(reader.read(&self.total_desired_fuel_id)); self.refuel_status = reader.read(&self.refuel_status_id); self.refuel_rate_setting = reader.read(&self.refuel_rate_setting_id); - self.ground_speed = reader.read(&self.ground_speed_id); - self.is_on_ground = reader.read(&self.is_on_ground_id); + for (id, state) in self .engine_state_ids .iter() @@ -216,8 +205,8 @@ impl IntegratedRefuelPanel { self.input.refuel_rate() } - fn refuel_is_enabled(&self) -> bool { - self.input.refuel_is_enabled() + fn refuel_is_enabled(&self, context: &UpdateContext) -> bool { + self.input.refuel_is_enabled(context) } fn target_zero_fuel_weight(&self) -> Mass { @@ -267,14 +256,13 @@ impl RefuelApplication { refuel_panel_input.target_zero_fuel_weight_cg_mac(), ); - // TODO: Uncomment when IS_SIM_READY variable is implemented - // if !context.is_sim_ready() { - // refuel_panel_input.set_fuel_desired(fuel_system.total_load()); - //} + if !context.is_sim_ready() { + refuel_panel_input.set_fuel_desired(fuel_system.total_load()); + } match refuel_panel_input.refuel_rate() { RefuelRate::Real => { - if refuel_panel_input.refuel_is_enabled() { + if refuel_panel_input.refuel_is_enabled(context) { self.refuel_driver.execute_timed_refuel( context.delta(), false, @@ -285,7 +273,7 @@ impl RefuelApplication { } } RefuelRate::Fast => { - if refuel_panel_input.refuel_is_enabled() { + if refuel_panel_input.refuel_is_enabled(context) { self.refuel_driver.execute_timed_refuel( context.delta(), true, diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/autobrakes.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/autobrakes.rs index 42f5a896563..968998acad3 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/autobrakes.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/autobrakes.rs @@ -500,7 +500,6 @@ impl A380AutobrakeController { lgciu1: &impl LgciuInterface, lgciu2: &impl LgciuInterface, placeholder_ground_spoilers_out: bool, - ground_speed: Velocity, ) { self.update_input_conditions( context, @@ -513,7 +512,6 @@ impl A380AutobrakeController { self.braking_distance_calculator.update_braking_estimations( context, - ground_speed, if self.mode == A380AutobrakeMode::BTV { self.btv_scheduler.predicted_decel() } else if self.mode != A380AutobrakeMode::DISARM { @@ -558,13 +556,12 @@ impl A380AutobrakeController { context, self.ground_spoilers_are_deployed, &self.braking_distance_calculator, - ground_speed, &self.autobrake_runway_overrun_protection, ); self.autobrake_runway_overrun_protection.update( + context, self.deceleration_governor.is_engaged(), - ground_speed, &self.braking_distance_calculator, lgciu1, lgciu2, @@ -618,7 +615,6 @@ struct AutobrakeRunwayOverrunProtection { is_actively_braking: bool, is_any_autobrake_active: bool, - ground_speed: Velocity, status_word: Arinc429Word, } @@ -647,15 +643,13 @@ impl AutobrakeRunwayOverrunProtection { is_any_autobrake_active: false, - ground_speed: Velocity::default(), - status_word: Arinc429Word::new(0, SignStatus::NormalOperation), } } - fn is_row_rop_operative(&self) -> bool { + fn is_row_rop_operative(&self, context: &UpdateContext) -> bool { self.distance_to_runway_end.is_normal_operation() - && self.ground_speed.get::() > Self::MIN_ARMING_SPEED_MS2 + && context.ground_speed().get::() > Self::MIN_ARMING_SPEED_MS2 } fn distance_to_runway_end(&self) -> Length { @@ -668,14 +662,13 @@ impl AutobrakeRunwayOverrunProtection { fn update( &mut self, + context: &UpdateContext, is_any_autobrake_active: bool, - ground_speed: Velocity, braking_distances: &BrakingDistanceCalculator, lgciu1: &impl LgciuInterface, lgciu2: &impl LgciuInterface, ) { self.is_any_autobrake_active = is_any_autobrake_active; - self.ground_speed = ground_speed; let is_on_ground = lgciu1.left_and_right_gear_compressed(false) || lgciu2.left_and_right_gear_compressed(false); @@ -683,7 +676,7 @@ impl AutobrakeRunwayOverrunProtection { let max_braking_prediction = braking_distances.max_braking(); // Can engage only above min speed - if self.is_row_rop_operative() && self.is_any_autobrake_active { + if self.is_row_rop_operative(context) && self.is_any_autobrake_active { if max_braking_prediction >= self.distance_to_runway_end.value() { self.is_actively_braking = true; } @@ -696,7 +689,8 @@ impl AutobrakeRunwayOverrunProtection { } // IS operative - self.status_word.set_bit(11, self.is_row_rop_operative()); + self.status_word + .set_bit(11, self.is_row_rop_operative(context)); // Is active under autobrake self.status_word.set_bit(12, self.is_actively_braking); @@ -704,10 +698,10 @@ impl AutobrakeRunwayOverrunProtection { // Is active under manual braking self.status_word.set_bit( 13, - self.should_show_manual_braking_warning(max_braking_prediction, is_on_ground), + self.should_show_manual_braking_warning(context, max_braking_prediction, is_on_ground), ); - let should_show_in_flight_row = !is_on_ground && self.is_row_rop_operative(); + let should_show_in_flight_row = !is_on_ground && self.is_row_rop_operative(context); // Too short if wet self.status_word.set_bit( 14, @@ -729,6 +723,7 @@ impl AutobrakeRunwayOverrunProtection { fn should_show_manual_braking_warning( &self, + context: &UpdateContext, dry_stopping_prediction: Length, is_on_ground: bool, ) -> bool { @@ -737,7 +732,7 @@ impl AutobrakeRunwayOverrunProtection { if is_on_ground && !any_engine_not_idle_or_reverse && !self.is_any_autobrake_active - && self.is_row_rop_operative() + && self.is_row_rop_operative(context) { dry_stopping_prediction >= self.distance_to_runway_end.value() } else { @@ -827,24 +822,21 @@ impl BrakingDistanceCalculator { } } - fn update_braking_estimations( - &mut self, - context: &UpdateContext, - ground_speed: Velocity, - deceleration: Acceleration, - ) { + fn update_braking_estimations(&mut self, context: &UpdateContext, deceleration: Acceleration) { // TODO use correct input to switch speed used let speed_used_for_prediction = if context.plane_height_over_ground().get::() < Self::ALTITUDE_THRESHOLD_TO_SWITCH_ESTIMATION_TO_GROUND_SPEED_FT { - ground_speed + context.ground_speed() } else { self.predicted_touchdown_speed.max(Velocity::new::( Self::MIN_PREDICTED_TOUCHDOWN_SPEED_KNOT, )) }; - if ground_speed.get::() > Self::MIN_SPEED_FOR_STOPPING_ESTIMATION_MS { + if context.ground_speed().get::() + > Self::MIN_SPEED_FOR_STOPPING_ESTIMATION_MS + { self.wet_landing_estimated_distance.update( context.delta(), self.stopping_distance_estimation_for_wet(speed_used_for_prediction), @@ -860,16 +852,17 @@ impl BrakingDistanceCalculator { if context.long_accel().get::() < Self::MIN_DECEL_FOR_STOPPING_ESTIMATION_MS2 - && ground_speed.get::() > Self::MIN_SPEED_FOR_STOPPING_ESTIMATION_MS + && context.ground_speed().get::() + > Self::MIN_SPEED_FOR_STOPPING_ESTIMATION_MS { self.braking_estimated_distance_at_current_decel.update( context.delta(), - self.stopping_distance_estimation_for_decel(ground_speed, deceleration), + self.stopping_distance_estimation_for_decel(context.ground_speed(), deceleration), ); self.braking_estimated_distance_at_max_decel.update( context.delta(), self.stopping_distance_estimation_for_decel( - ground_speed, + context.ground_speed(), Acceleration::new::(Self::MAX_DECEL_DRY_MS2), ), ); @@ -1090,7 +1083,6 @@ impl BtvDecelScheduler { context: &UpdateContext, spoilers_active: bool, braking_distance: &BrakingDistanceCalculator, - ground_speed: Velocity, rop: &AutobrakeRunwayOverrunProtection, ) { self.distance_to_rwy_end = rop.distance_to_runway_end(); @@ -1101,11 +1093,11 @@ impl BtvDecelScheduler { self.spoilers_active = spoilers_active; self.actual_deceleration = context.long_accel(); - self.integrate_distance(context, ground_speed); + self.integrate_distance(context); - self.compute_decel(ground_speed); + self.compute_decel(context); - self.state = self.update_state(ground_speed); + self.state = self.update_state(context); } fn braking_distance_remaining(&self) -> Length { @@ -1126,7 +1118,7 @@ impl BtvDecelScheduler { .clamp(Length::default(), distance_to_runway_end_minus_margin) } - fn compute_decel(&mut self, ground_speed: Velocity) { + fn compute_decel(&mut self, context: &UpdateContext) { match self.state { BTVState::RotOptimization | BTVState::Decel | BTVState::EndOfBraking => { let speed_at_btv_release = @@ -1135,7 +1127,7 @@ impl BtvDecelScheduler { self.final_distance_remaining = self.braking_distance_remaining(); - let delta_speed_to_achieve = ground_speed - speed_at_btv_release; + let delta_speed_to_achieve = context.ground_speed() - speed_at_btv_release; let target_deceleration_raw = -delta_speed_to_achieve.get::().powi(2) @@ -1193,7 +1185,7 @@ impl BtvDecelScheduler { } } - fn update_state(&mut self, ground_speed: Velocity) -> BTVState { + fn update_state(&mut self, context: &UpdateContext) -> BTVState { match self.state { BTVState::Armed => { if self.spoilers_active { @@ -1218,7 +1210,7 @@ impl BtvDecelScheduler { } BTVState::Decel => { if self.final_distance_remaining.get::() < Self::DISTANCE_TO_RELEASE_BTV_M - || ground_speed.get::() + || context.ground_speed().get::() <= Self::TARGET_SPEED_TO_RELEASE_BTV_M_S { self.end_of_decel_acceleration = self.deceleration_request; @@ -1228,7 +1220,9 @@ impl BtvDecelScheduler { } } BTVState::EndOfBraking => { - if ground_speed.get::() <= Self::TARGET_SPEED_TO_RELEASE_BTV_M_S { + if context.ground_speed().get::() + <= Self::TARGET_SPEED_TO_RELEASE_BTV_M_S + { self.disarm(); BTVState::Disabled } else { @@ -1239,10 +1233,10 @@ impl BtvDecelScheduler { } } - fn integrate_distance(&mut self, context: &UpdateContext, ground_speed: Velocity) { + fn integrate_distance(&mut self, context: &UpdateContext) { match self.state { BTVState::RotOptimization | BTVState::Decel | BTVState::EndOfBraking => { - let distance_this_tick = ground_speed * context.delta_as_time(); + let distance_this_tick = context.ground_speed() * context.delta_as_time(); self.rolling_distance += distance_this_tick; } @@ -1376,7 +1370,7 @@ impl SimulationElement for BtvDecelScheduler { #[cfg(test)] mod braking_distance_tests { - use systems::simulation::test::TestBed; + use systems::simulation::test::{TestBed, WriteByName}; use super::*; use crate::systems::simulation::test::{ElementCtorFn, SimulationTestBed}; @@ -1385,14 +1379,11 @@ mod braking_distance_tests { fn landing_140_knot_dry_line() { let mut test_bed = SimulationTestBed::from(ElementCtorFn(BrakingDistanceCalculator::new)) .with_update_after_power_distribution(|e, context| { - e.update_braking_estimations( - context, - Velocity::new::(140.), - Acceleration::default(), - ) + e.update_braking_estimations(context, Acceleration::default()) }); test_bed.set_on_ground(true); + test_bed.write_by_name("GPS GROUND SPEED", 140.); test_bed.run_multiple_frames(Duration::from_secs(5)); assert!( @@ -1405,13 +1396,10 @@ mod braking_distance_tests { fn landing_140_knot_wet_line() { let mut test_bed = SimulationTestBed::from(ElementCtorFn(BrakingDistanceCalculator::new)) .with_update_after_power_distribution(|e, context| { - e.update_braking_estimations( - context, - Velocity::new::(140.), - Acceleration::default(), - ) + e.update_braking_estimations(context, Acceleration::default()) }); + test_bed.write_by_name("GPS GROUND SPEED", 140.); test_bed.run_multiple_frames(Duration::from_secs(5)); assert!( diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/heading_control_steering.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/heading_control_steering.rs new file mode 100644 index 00000000000..c20dfcf566a --- /dev/null +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/heading_control_steering.rs @@ -0,0 +1,195 @@ +use systems::{ + shared::{low_pass_filter::LowPassFilter, pid::PidController, AdirsMeasurementOutputs}, + simulation::UpdateContext, +}; + +use std::time::Duration; +use uom::si::{angle::degree, f64::*, velocity::knot}; + +#[derive(Debug)] +enum HCFState { + CaptureHeading, + Tracking, + Disabled, +} + +pub(crate) struct HeadingControlFunction { + state: HCFState, + + tracked_heading: Option, + new_heading: Option, + previous_heading: Option, + + yaw_rate: LowPassFilter, + oloop_gain: f64, + + steering_controller: PidController, + + steering_output: Angle, +} +impl HeadingControlFunction { + const MAX_SPEED_KNOT: f64 = 50.; + const MAX_ALLOWED_ANGLE_BETWEEN_FRAMES_DEGREES: f64 = 5.; + + const MAX_YAW_RATE_ALLOWING_TRACKING_ENTRY_DEGREE_PER_S: f64 = 0.1; + + pub fn default() -> Self { + Self { + state: HCFState::Disabled, + + tracked_heading: None, + new_heading: None, + previous_heading: None, + + yaw_rate: LowPassFilter::new(Duration::from_millis(200)), + oloop_gain: 6., + + steering_controller: PidController::new(5., 0.3, 5., -3., 3., 0., 1.), + steering_output: Angle::default(), + } + } + + fn update_state( + &mut self, + context: &UpdateContext, + nose_steering_feedback: Angle, + is_any_steering_input: bool, + ) { + match self.state { + HCFState::Disabled => { + if self.hcf_available(context) && !self.should_reset(context) { + self.state = HCFState::CaptureHeading; + } + } + HCFState::CaptureHeading => { + if self.should_reset(context) || !self.hcf_available(context) { + self.state = HCFState::Disabled; + self.tracked_heading = None; + self.new_heading = None; + } else if self.yaw_rate.output().abs() + < Self::MAX_YAW_RATE_ALLOWING_TRACKING_ENTRY_DEGREE_PER_S + && !is_any_steering_input + { + self.state = HCFState::Tracking; + } + } + HCFState::Tracking => { + if self.should_reset(context) || !self.hcf_available(context) { + self.state = HCFState::Disabled; + self.tracked_heading = None; + self.new_heading = None; + } else if nose_steering_feedback.get::().abs() > 3.5 + || is_any_steering_input + { + self.state = HCFState::CaptureHeading + } + } + } + } + + fn states_actions( + &mut self, + context: &UpdateContext, + nose_steering_feedback: Angle, + is_any_steering_input: bool, + ) { + match self.state { + HCFState::Disabled => self.reset(), + HCFState::CaptureHeading => { + self.tracked_heading = self.new_heading; + + // If no input, and steering close to center openloop mode tries to stop yaw rate + self.steering_output = if !is_any_steering_input + && nose_steering_feedback.get::().abs() < 3.5 + { + Angle::new::( + (-self.oloop_gain + * self.yaw_rate.output().abs().powf(1.4) + * self.yaw_rate.output().signum()) + .clamp(-3., 3.), + ) + } else { + Angle::default() + }; + self.steering_controller + .reset_with_output(self.steering_output.get::()); + } + HCFState::Tracking => { + self.steering_controller.change_setpoint( + self.normalize_angle(self.tracked_heading.unwrap().get::()), + ); + self.steering_output = + Angle::new::(self.steering_controller.next_control_output( + self.normalize_angle(self.new_heading.unwrap().get::()), + Some(context.delta()), + )); + } + } + } + + fn hcf_available(&self, context: &UpdateContext) -> bool { + context.ground_speed().get::() < Self::MAX_SPEED_KNOT + } + + fn should_reset(&self, context: &UpdateContext) -> bool { + !self.heading_refresh_is_valid() + || context.is_sim_initialiazing() + || !context.is_sim_ready() + } + + pub fn update( + &mut self, + context: &UpdateContext, + adir_in_use: usize, + adirs: &impl AdirsMeasurementOutputs, + is_any_steering_input: bool, + nose_steering_feedback: Angle, + ) { + self.previous_heading = self.new_heading; + self.new_heading = adirs.true_heading(adir_in_use).normal_value(); + + if self.heading_refresh_is_valid() { + let new = self.normalize_angle(self.new_heading.unwrap_or_default().get::()); + let prev = + self.normalize_angle(self.previous_heading.unwrap_or_default().get::()); + self.yaw_rate + .update(context.delta(), (new - prev) / context.delta_as_secs_f64()); + } + + self.update_state(context, nose_steering_feedback, is_any_steering_input); + + self.states_actions(context, nose_steering_feedback, is_any_steering_input); + } + + fn heading_refresh_is_valid(&self) -> bool { + if self.new_heading.is_none() || self.previous_heading.is_none() { + false + } else { + let new_angle_degree = self.new_heading.unwrap().get::(); + + let angle_diff = (self.normalize_angle(new_angle_degree) + - self.normalize_angle(self.previous_heading.unwrap().get::())) + .abs(); + + angle_diff < Self::MAX_ALLOWED_ANGLE_BETWEEN_FRAMES_DEGREES + } + } + + pub fn steering_output(&self) -> Angle { + self.steering_output + } + + fn reset(&mut self) { + self.steering_output = Angle::default(); + self.steering_controller.reset_with_output(0.); + } + + // Normallize all angles in degrees in the -180;180 range + fn normalize_angle(&self, angle: f64) -> f64 { + let mut normalized = (angle + 180.0) % 360.0; + if normalized < 0.0 { + normalized += 360.0; + } + normalized - 180.0 + } +} diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/mod.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/mod.rs index 63a5e787304..4f44bd9ae92 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/mod.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/hydraulic/mod.rs @@ -3,7 +3,7 @@ use nalgebra::Vector3; use std::time::Duration; use uom::si::{ angle::degree, - angular_velocity::{radian_per_second, revolution_per_minute}, + angular_velocity::{degree_per_second, radian_per_second, revolution_per_minute}, electric_current::ampere, f64::*, length::meter, @@ -67,6 +67,8 @@ use engine_pump_disc::EnginePumpDisconnectionClutch; pub mod autobrakes; use autobrakes::A380AutobrakeController; mod gear_secondary_doors; +mod heading_control_steering; +use heading_control_steering::HeadingControlFunction; #[cfg(test)] use systems::hydraulic::PressureSwitchState; @@ -1500,6 +1502,8 @@ impl A380GearSystemFactory { pub(super) struct A380Hydraulic { nose_steering: SteeringActuator, + body_wheel_steering_left: SteeringActuator, + body_wheel_steering_right: SteeringActuator, core_hydraulic_updater: MaxStepLoop, @@ -1640,11 +1644,35 @@ impl A380Hydraulic { A380Hydraulic { nose_steering: SteeringActuator::new( context, + "NOSE_WHEEL", Angle::new::(75.), - AngularVelocity::new::(0.35), + AngularVelocity::new::(15.), // Reference is 15 deg/s Length::new::(0.11), // Diameter of 0.11 gives correct A380 flow of around 35 lpm at full speed Ratio::new::(0.18), Pressure::new::(4000.), + true, + ), + + body_wheel_steering_left: SteeringActuator::new( + context, + "LEFT_BODY_STEERING", + Angle::new::(15.), + AngularVelocity::new::(4.5), // Reference is 4.5 deg/s + Length::new::(0.11), // TODO + Ratio::new::(0.18), // TODO + Pressure::new::(4000.), + false, + ), + + body_wheel_steering_right: SteeringActuator::new( + context, + "RIGHT_BODY_STEERING", + Angle::new::(15.), + AngularVelocity::new::(4.5), // Reference is 4.5 deg/s + Length::new::(0.11), // TODO + Ratio::new::(0.18), // TODO + Pressure::new::(4000.), + false, ), core_hydraulic_updater: MaxStepLoop::new(Self::HYDRAULIC_SIM_TIME_STEP), @@ -2229,6 +2257,21 @@ impl A380Hydraulic { &self.bypass_pin, ); + self.body_wheel_steering_left.update( + context, + self.yellow_circuit.system_section(), + self.brake_steer_computer.left_body_steering_controller(), + &self.pushback_tug, + &self.bypass_pin, + ); + self.body_wheel_steering_right.update( + context, + self.yellow_circuit.system_section(), + self.brake_steer_computer.right_body_steering_controller(), + &self.pushback_tug, + &self.bypass_pin, + ); + // Process brake logic (which circuit brakes) and send brake demands (how much) self.brake_steer_computer.update( context, @@ -2242,6 +2285,7 @@ impl A380Hydraulic { adirs, self.left_spoilers.ground_spoilers_are_requested() && self.right_spoilers.ground_spoilers_are_requested(), + self.nose_steering.position_feedback(), ); self.pushback_tug.update(context); @@ -2294,7 +2338,8 @@ impl A380Hydraulic { context, &self.forward_cargo_door_controller, &self.aft_cargo_door_controller, - &self.bypass_pin, + self.brake_steer_computer + .should_pressurise_for_body_steering(), overhead_panel, ); @@ -2477,6 +2522,12 @@ impl A380Hydraulic { self.yellow_circuit .update_system_actuator_volumes(&mut self.nose_steering); + self.yellow_circuit + .update_system_actuator_volumes(&mut self.body_wheel_steering_left); + + self.yellow_circuit + .update_system_actuator_volumes(&mut self.body_wheel_steering_right); + self.yellow_circuit .update_system_actuator_volumes(self.right_elevator.actuator( ElevatorActuatorPosition::Outward, @@ -2936,6 +2987,9 @@ impl SimulationElement for A380Hydraulic { self.braking_force.accept(visitor); self.nose_steering.accept(visitor); + self.body_wheel_steering_left.accept(visitor); + self.body_wheel_steering_right.accept(visitor); + self.slats_flaps_complex.accept(visitor); self.flap_system.accept(visitor); self.slat_system.accept(visitor); @@ -3450,7 +3504,7 @@ impl A380ElectricPumpAutoLogic { Duration::from_secs(20); const DURATION_OF_PUMP_ACTIVATION_AFTER_BODY_STEERING_OPERATION: Duration = - Duration::from_secs(5); + Duration::from_secs(30); fn new( green_a_pump_powered_by: ElectricalBusType, green_b_pump_powered_by: ElectricalBusType, @@ -3488,14 +3542,14 @@ impl A380ElectricPumpAutoLogic { context: &UpdateContext, forward_cargo_door_controller: &HydraulicDoorController, aft_cargo_door_controller: &HydraulicDoorController, - bypass_pin: &BypassPin, + is_body_steering_active: bool, overhead: &A380HydraulicOverheadPanel, ) { self.update_auto_run_logic( context, forward_cargo_door_controller, aft_cargo_door_controller, - bypass_pin, + is_body_steering_active, ); self.select_pump_in_use(overhead); @@ -3506,7 +3560,7 @@ impl A380ElectricPumpAutoLogic { context: &UpdateContext, forward_cargo_door_controller: &HydraulicDoorController, aft_cargo_door_controller: &HydraulicDoorController, - bypass_pin: &BypassPin, + is_body_steering_active: bool, ) { self.cargo_door_in_operation_previous = self.is_required_for_cargo_door_operation.output(); @@ -3520,7 +3574,7 @@ impl A380ElectricPumpAutoLogic { self.is_required_for_body_steering_operation.output(); self.is_required_for_body_steering_operation - .update(context, bypass_pin.is_nose_wheel_steering_pin_inserted()); + .update(context, is_body_steering_active); } fn select_pump_in_use(&mut self, overhead: &A380HydraulicOverheadPanel) { @@ -3824,6 +3878,137 @@ impl BrakeCircuitController for A380BrakeSystemOutputs { } } +struct BodyWheelSteeringController { + requested_position: Angle, + is_left_side: bool, +} +impl BodyWheelSteeringController { + const NOSE_ANGLE_INPUT_DEGREES: [f64; 4] = [-70., -20., 20., 70.]; + const BODY_STEERING_DEMAND_DEGREES: [f64; 4] = [15., 0., 0., -11.]; + + const MAX_BODY_STEERING_SPEED_ENABLING_KNOT: f64 = 30.; + + fn new(is_left_side: bool) -> Self { + Self { + requested_position: Angle::default(), + is_left_side, + } + } + + fn update(&mut self, context: &UpdateContext, nose_steering_angle: Angle) { + if context.ground_speed().get::() < Self::MAX_BODY_STEERING_SPEED_ENABLING_KNOT { + self.requested_position = if self.is_left_side { + Angle::new::(interpolation( + &Self::NOSE_ANGLE_INPUT_DEGREES, + &Self::BODY_STEERING_DEMAND_DEGREES, + nose_steering_angle.get::(), + )) + } else { + -Angle::new::(interpolation( + &Self::NOSE_ANGLE_INPUT_DEGREES, + &Self::BODY_STEERING_DEMAND_DEGREES, + -nose_steering_angle.get::(), + )) + }; + } else { + self.requested_position = Angle::default(); + } + } +} +impl SteeringController for BodyWheelSteeringController { + fn requested_position(&self) -> Angle { + self.requested_position + } +} + +struct BodyWheelSteeringControl { + left_controller: BodyWheelSteeringController, + right_controller: BodyWheelSteeringController, +} +impl BodyWheelSteeringControl { + fn default() -> Self { + Self { + left_controller: BodyWheelSteeringController::new(true), + right_controller: BodyWheelSteeringController::new(false), + } + } + + fn update(&mut self, context: &UpdateContext, nose_steering_feedback: Angle) { + self.left_controller.update(context, nose_steering_feedback); + self.right_controller + .update(context, nose_steering_feedback); + } + + fn left_controller(&self) -> &impl SteeringController { + &self.left_controller + } + + fn right_controller(&self) -> &impl SteeringController { + &self.right_controller + } + + fn is_requesting_steering(&self) -> bool { + self.left_controller + .requested_position() + .get::() + .abs() + > 0.1 + || self + .right_controller + .requested_position() + .get::() + .abs() + > 0.1 + } +} + +struct PedalSteeringDynamicLimiter { + pedal_steering_limiter_landing: SteeringAngleLimiter<5>, + pedal_steering_limiter_takeoff: SteeringAngleLimiter<5>, + is_landing_mode: bool, +} +impl PedalSteeringDynamicLimiter { + const SPEED_MAP_FOR_PEDAL_ACTION_KNOT: [f64; 5] = [0., 100., 150., 1500.0, 2800.0]; + const STEERING_ANGLE_FOR_PEDAL_ACTION_TAKEOFF_RATIO: [f64; 5] = [1., 1., 0.333, 0.333, 0.333]; // Used in takeoff mode only + const STEERING_ANGLE_FOR_PEDAL_ACTION_LANDING_RATIO: [f64; 5] = [1., 1., 0., 0., 0.]; // Used in landing mode only + const SPEED_TO_SWITCH_TAKEOFF_MODE_KNOT: f64 = 100.; + + fn default() -> Self { + Self { + pedal_steering_limiter_landing: SteeringAngleLimiter::new( + Self::SPEED_MAP_FOR_PEDAL_ACTION_KNOT, + Self::STEERING_ANGLE_FOR_PEDAL_ACTION_LANDING_RATIO, + ), + pedal_steering_limiter_takeoff: SteeringAngleLimiter::new( + Self::SPEED_MAP_FOR_PEDAL_ACTION_KNOT, + Self::STEERING_ANGLE_FOR_PEDAL_ACTION_TAKEOFF_RATIO, + ), + is_landing_mode: false, + } + } + + fn update(&mut self, ground_speed: Velocity, lgciu: &impl LgciuWeightOnWheels) { + if !lgciu.left_and_right_gear_compressed(false) { + self.is_landing_mode = true; + } + if ground_speed.get::() < Self::SPEED_TO_SWITCH_TAKEOFF_MODE_KNOT + && lgciu.left_and_right_gear_compressed(false) + { + self.is_landing_mode = false; + } + } + + fn angle_from_speed(&self, speed: Velocity, angle_demand: Angle) -> Angle { + if self.is_landing_mode { + self.pedal_steering_limiter_landing + .angle_from_speed(speed, angle_demand) + } else { + self.pedal_steering_limiter_takeoff + .angle_from_speed(speed, angle_demand) + } + } +} + struct A380HydraulicBrakeSteerComputerUnit { park_brake_lever_pos_id: VariableIdentifier, @@ -3831,8 +4016,6 @@ struct A380HydraulicBrakeSteerComputerUnit { left_brake_pedal_input_id: VariableIdentifier, right_brake_pedal_input_id: VariableIdentifier, - ground_speed_id: VariableIdentifier, - rudder_pedal_input_id: VariableIdentifier, tiller_handle_input_id: VariableIdentifier, tiller_pedal_disconnect_id: VariableIdentifier, @@ -3856,13 +4039,15 @@ struct A380HydraulicBrakeSteerComputerUnit { rudder_pedal_position: Ratio, autopilot_nosewheel_demand: Ratio, - pedal_steering_limiter: SteeringAngleLimiter<5>, + pedal_steering_limiter: PedalSteeringDynamicLimiter, pedal_input_map: SteeringRatioToAngle<6>, tiller_steering_limiter: SteeringAngleLimiter<5>, tiller_input_map: SteeringRatioToAngle<6>, final_steering_position_request: Angle, - ground_speed: Velocity, + body_wheel_steering_control: BodyWheelSteeringControl, + + heading_control_function: HeadingControlFunction, } impl A380HydraulicBrakeSteerComputerUnit { const RUDDER_PEDAL_INPUT_GAIN: f64 = 32.; @@ -3870,19 +4055,16 @@ impl A380HydraulicBrakeSteerComputerUnit { const RUDDER_PEDAL_INPUT_CURVE_MAP: [f64; 6] = [0., 0., 2., 6.4, 6.4, 6.4]; const MAX_RUDDER_INPUT_INCLUDING_AUTOPILOT_DEGREE: f64 = 6.; - const SPEED_MAP_FOR_PEDAL_ACTION_KNOT: [f64; 5] = [0., 40., 130., 1500.0, 2800.0]; - const STEERING_ANGLE_FOR_PEDAL_ACTION_DEGREE: [f64; 5] = [1., 1., 0., 0., 0.]; - const TILLER_INPUT_GAIN: f64 = 75.; const TILLER_INPUT_MAP: [f64; 6] = [0., 1., 20., 40., 66., 75.]; const TILLER_INPUT_CURVE_MAP: [f64; 6] = [0., 0., 4., 15., 45., 74.]; const AUTOPILOT_STEERING_INPUT_GAIN: f64 = 6.; - const SPEED_MAP_FOR_TILLER_ACTION_KNOT: [f64; 5] = [0., 20., 70., 1500.0, 2800.0]; - const STEERING_ANGLE_FOR_TILLER_ACTION_DEGREE: [f64; 5] = [1., 1., 0., 0., 0.]; + const SPEED_MAP_FOR_TILLER_ACTION_KNOT: [f64; 5] = [0., 40., 100., 1500.0, 2800.0]; + const STEERING_ANGLE_FOR_TILLER_ACTION_RATIO: [f64; 5] = [1., 1., 0., 0., 0.]; - const MAX_STEERING_ANGLE_DEMAND_DEGREES: f64 = 74.; + const MAX_STEERING_ANGLE_DEMAND_DEGREES: f64 = 70.; // Minimum pressure hysteresis on green until main switched on ALTN brakes // Feedback by Cpt. Chaos — 25/04/2021 #pilot-feedback @@ -3905,7 +4087,6 @@ impl A380HydraulicBrakeSteerComputerUnit { right_brake_pedal_input_id: context .get_identifier("RIGHT_BRAKE_PEDAL_INPUT".to_owned()), - ground_speed_id: context.get_identifier("GPS GROUND SPEED".to_owned()), rudder_pedal_input_id: context.get_identifier("RUDDER_PEDAL_POSITION_RATIO".to_owned()), tiller_handle_input_id: context.get_identifier("TILLER_HANDLE_POSITION".to_owned()), tiller_pedal_disconnect_id: context @@ -3931,10 +4112,7 @@ impl A380HydraulicBrakeSteerComputerUnit { rudder_pedal_position: Ratio::new::(0.), autopilot_nosewheel_demand: Ratio::new::(0.), - pedal_steering_limiter: SteeringAngleLimiter::new( - Self::SPEED_MAP_FOR_PEDAL_ACTION_KNOT, - Self::STEERING_ANGLE_FOR_PEDAL_ACTION_DEGREE, - ), + pedal_steering_limiter: PedalSteeringDynamicLimiter::default(), pedal_input_map: SteeringRatioToAngle::new( Ratio::new::(Self::RUDDER_PEDAL_INPUT_GAIN), Self::RUDDER_PEDAL_INPUT_MAP, @@ -3942,7 +4120,7 @@ impl A380HydraulicBrakeSteerComputerUnit { ), tiller_steering_limiter: SteeringAngleLimiter::new( Self::SPEED_MAP_FOR_TILLER_ACTION_KNOT, - Self::STEERING_ANGLE_FOR_TILLER_ACTION_DEGREE, + Self::STEERING_ANGLE_FOR_TILLER_ACTION_RATIO, ), tiller_input_map: SteeringRatioToAngle::new( Ratio::new::(Self::TILLER_INPUT_GAIN), @@ -3951,7 +4129,9 @@ impl A380HydraulicBrakeSteerComputerUnit { ), final_steering_position_request: Angle::new::(0.), - ground_speed: Velocity::new::(0.), + body_wheel_steering_control: BodyWheelSteeringControl::default(), + + heading_control_function: HeadingControlFunction::default(), } } @@ -4017,8 +4197,19 @@ impl A380HydraulicBrakeSteerComputerUnit { engine2: &impl Engine, adirs: &impl AdirsMeasurementOutputs, placeholder_ground_spoilers_out: bool, + nose_steering_feedback: Angle, ) { - self.update_steering_demands(lgciu1, engine1, engine2); + // TODO split steering part from braking part in two different computers instances + self.update_steering_demands( + context, + adirs, + lgciu1, + engine1, + engine2, + nose_steering_feedback, + ); + self.body_wheel_steering_control + .update(context, nose_steering_feedback); self.update_normal_braking_availability(current_pressure.pressure()); self.update_brake_pressure_limitation(); @@ -4032,7 +4223,6 @@ impl A380HydraulicBrakeSteerComputerUnit { lgciu1, lgciu2, placeholder_ground_spoilers_out, - self.ground_speed, ); let is_in_flight_gear_lever_up = !(lgciu1.left_and_right_gear_compressed(true) @@ -4098,10 +4288,16 @@ impl A380HydraulicBrakeSteerComputerUnit { fn update_steering_demands( &mut self, + context: &UpdateContext, + adirs: &impl AdirsMeasurementOutputs, lgciu1: &impl LgciuInterface, engine1: &impl Engine, engine2: &impl Engine, + nose_steering_feedback: Angle, ) { + self.pedal_steering_limiter + .update(context.ground_speed(), lgciu1); // TODO check which lgciu input is used for mode determination + let steer_angle_from_autopilot = Angle::new::( self.autopilot_nosewheel_demand.get::() * Self::AUTOPILOT_STEERING_INPUT_GAIN, ); @@ -4115,7 +4311,7 @@ impl A380HydraulicBrakeSteerComputerUnit { // TODO Here ground speed would be probably computed from wheel sensor logic let final_steer_rudder_plus_autopilot = self.pedal_steering_limiter.angle_from_speed( - self.ground_speed, + context.ground_speed(), (steer_angle_from_pedals + steer_angle_from_autopilot) .min(Angle::new::( Self::MAX_RUDDER_INPUT_INCLUDING_AUTOPILOT_DEGREE, @@ -4126,11 +4322,24 @@ impl A380HydraulicBrakeSteerComputerUnit { ); let steer_angle_from_tiller = self.tiller_steering_limiter.angle_from_speed( - self.ground_speed, + context.ground_speed(), self.tiller_input_map .angle_demand_from_input_demand(self.tiller_handle_position), ); + let any_steering_demand_active = steer_angle_from_autopilot.abs() + > Angle::new::(0.) + || steer_angle_from_pedals.abs() > Angle::new::(0.1) + || steer_angle_from_tiller.abs() > Angle::new::(0.1); + + self.heading_control_function.update( + context, + 1, + adirs, + any_steering_demand_active, + nose_steering_feedback, + ); + let is_both_engine_low_oil_pressure = engine1.oil_pressure_is_low() && engine2.oil_pressure_is_low(); @@ -4138,13 +4347,15 @@ impl A380HydraulicBrakeSteerComputerUnit { && self.anti_skid_activated && lgciu1.nose_gear_compressed(false) { - (final_steer_rudder_plus_autopilot + steer_angle_from_tiller) - .min(Angle::new::( - Self::MAX_STEERING_ANGLE_DEMAND_DEGREES, - )) - .max(Angle::new::( - -Self::MAX_STEERING_ANGLE_DEMAND_DEGREES, - )) + (final_steer_rudder_plus_autopilot + + steer_angle_from_tiller + + self.heading_control_function.steering_output()) + .min(Angle::new::( + Self::MAX_STEERING_ANGLE_DEMAND_DEGREES, + )) + .max(Angle::new::( + -Self::MAX_STEERING_ANGLE_DEMAND_DEGREES, + )) } else { Angle::new::(0.) }; @@ -4157,6 +4368,18 @@ impl A380HydraulicBrakeSteerComputerUnit { fn alternate_controller(&self) -> &impl BrakeCircuitController { &self.alternate_brake_outputs } + + fn left_body_steering_controller(&self) -> &impl SteeringController { + self.body_wheel_steering_control.left_controller() + } + + fn right_body_steering_controller(&self) -> &impl SteeringController { + self.body_wheel_steering_control.right_controller() + } + + fn should_pressurise_for_body_steering(&self) -> bool { + self.body_wheel_steering_control.is_requesting_steering() + } } impl SimulationElement for A380HydraulicBrakeSteerComputerUnit { fn accept(&mut self, visitor: &mut T) { @@ -4177,7 +4400,6 @@ impl SimulationElement for A380HydraulicBrakeSteerComputerUnit { Ratio::new::(reader.read(&self.tiller_handle_input_id)); self.rudder_pedal_position = Ratio::new::(reader.read(&self.rudder_pedal_input_id)); self.tiller_pedal_disconnect = reader.read(&self.tiller_pedal_disconnect_id); - self.ground_speed = reader.read(&self.ground_speed_id); self.autopilot_nosewheel_demand = Ratio::new::(reader.read(&self.autopilot_nosewheel_demand_id)); @@ -6554,7 +6776,7 @@ mod tests { }; use uom::si::{ - angle::{degree, radian}, + angle::degree, electric_potential::volt, length::foot, ratio::{percent, ratio}, @@ -7278,10 +7500,23 @@ mod tests { Ratio::new::(self.read_by_name("HYD_RUD_DEFLECTION")) } + fn set_rudder_input(mut self, steer_ratio: Ratio) -> Self { + self.write_by_name("RUDDER_PEDAL_POSITION_RATIO", steer_ratio.get::()); + self + } + fn get_nose_steering_ratio(&mut self) -> Ratio { Ratio::new::(self.read_by_name("NOSE_WHEEL_POSITION_RATIO")) } + fn get_left_body_steering_steering_ratio(&mut self) -> Ratio { + Ratio::new::(self.read_by_name("LEFT_BODY_STEERING_POSITION_RATIO")) + } + + fn get_right_body_steering_steering_ratio(&mut self) -> Ratio { + Ratio::new::(self.read_by_name("RIGHT_BODY_STEERING_POSITION_RATIO")) + } + fn _is_fire_valve_eng1_closed(&mut self) -> bool { !ReadByName::::read_by_name( self, @@ -7337,15 +7572,22 @@ mod tests { self.set_on_ground(false); self.set_indicated_altitude(Length::new::(2500.)); self.set_indicated_airspeed(Velocity::new::(180.)); + self.start_eng1(Ratio::new::(80.)) .start_eng2(Ratio::new::(80.)) .start_eng3(Ratio::new::(80.)) .start_eng4(Ratio::new::(80.)) + .set_ground_speed(Velocity::new::(180.)) .set_gear_lever_up() .set_park_brake(false) .external_power(false) } + fn set_ground_speed(mut self, ground_speed: Velocity) -> Self { + self.write_by_name("GPS GROUND SPEED", ground_speed.get::()); + self + } + fn adirs_not_aligned(mut self) -> Self { self.set_adirs_not_aligned(); self @@ -7405,8 +7647,10 @@ mod tests { self } - fn set_pushback_angle(mut self, angle: Angle) -> Self { - self.write_by_name("PUSHBACK ANGLE", angle.get::()); + fn set_pushback_angle(mut self, angle: AngularVelocity) -> Self { + self.write_by_name("ROTATION VELOCITY BODY Y", angle.get::()); + self.write_by_name("VELOCITY BODY Z", -1.); + self } @@ -10319,16 +10563,8 @@ mod tests { test_bed = test_bed .set_pushback_state(true) - .set_pushback_angle(Angle::new::(80.)) - .run_waiting_for(Duration::from_secs_f64(0.5)); - - // Do not turn instantly in 0.5s - assert!( - test_bed.get_nose_steering_ratio() > Ratio::new::(0.) - && test_bed.get_nose_steering_ratio() < Ratio::new::(0.5) - ); - - test_bed = test_bed.run_waiting_for(Duration::from_secs_f64(5.)); + .set_pushback_angle(AngularVelocity::new::(-5.)) + .run_waiting_for(Duration::from_secs_f64(5.)); // Has turned fully after 5s assert!(test_bed.get_nose_steering_ratio() > Ratio::new::(0.9)); @@ -10336,12 +10572,8 @@ mod tests { // Going left test_bed = test_bed .set_pushback_state(true) - .set_pushback_angle(Angle::new::(-80.)) - .run_waiting_for(Duration::from_secs_f64(0.5)); - - assert!(test_bed.get_nose_steering_ratio() > Ratio::new::(0.2)); - - test_bed = test_bed.run_waiting_for(Duration::from_secs_f64(5.)); + .set_pushback_angle(AngularVelocity::new::(5.)) + .run_waiting_for(Duration::from_secs_f64(5.)); // Has turned fully left after 5s assert!(test_bed.get_nose_steering_ratio() < Ratio::new::(-0.9)); @@ -10534,7 +10766,7 @@ mod tests { } #[test] - fn yellow_epump_buildup_system_section_when_pushback() { + fn yellow_epump_buildup_system_section_when_pushback_turns_nose_more_than_20_degrees() { let mut test_bed = test_bed_on_ground_with() .engines_off() .on_the_ground() @@ -10543,6 +10775,7 @@ mod tests { test_bed = test_bed .set_pushback_state(true) + .set_pushback_angle(AngularVelocity::new::(5.)) .run_waiting_for(Duration::from_secs(5)); assert!(!test_bed.is_green_pressure_switch_pressurised()); @@ -10636,5 +10869,135 @@ mod tests { assert!(test_bed.is_cargo_fwd_door_locked_up()); assert!(test_bed.is_cargo_aft_door_locked_up()); } + + #[test] + fn pushback_steering_full_left_turns_body_steering() { + let mut test_bed = test_bed_on_ground_with() + .engines_off() + .on_the_ground() + .set_cold_dark_inputs() + .run_one_tick(); + + test_bed = test_bed + .set_pushback_state(true) + .set_pushback_angle(AngularVelocity::new::(5.)) + .run_waiting_for(Duration::from_secs(15)); + + // Left BWS should be at max right steer (15°) for left turn + assert!( + test_bed + .get_left_body_steering_steering_ratio() + .get::() + >= 0.97 + ); + + // Right BWS should be at (11°) right steer for left turn + let expected_right_angle = 11. / 15.; + assert!( + test_bed + .get_right_body_steering_steering_ratio() + .get::() + < 0.9 + ); + assert!( + test_bed + .get_right_body_steering_steering_ratio() + .get::() + > expected_right_angle - 0.05 + ); + } + + #[test] + fn pushback_steering_full_right_turns_body_steering() { + let mut test_bed = test_bed_on_ground_with() + .engines_off() + .on_the_ground() + .set_cold_dark_inputs() + .run_one_tick(); + + test_bed = test_bed + .set_pushback_state(true) + .set_pushback_angle(AngularVelocity::new::(-5.)) + .run_waiting_for(Duration::from_secs(15)); + + // Right BWS should be at max left steer (15°) for right turn + assert!( + test_bed + .get_right_body_steering_steering_ratio() + .get::() + <= -0.97 + ); + + // Left BWS should be at (11°) left steer for right turn + let expected_right_angle = -11. / 15.; + assert!( + test_bed + .get_left_body_steering_steering_ratio() + .get::() + > -0.9 + ); + assert!( + test_bed + .get_left_body_steering_steering_ratio() + .get::() + < expected_right_angle + 0.05 + ); + } + + #[test] + fn takeoff_mode_limits_nose_steering_to_2_degrees_after_150_knot() { + let mut test_bed = test_bed_on_ground_with() + .start_eng1(Ratio::new::(95.)) + .start_eng2(Ratio::new::(95.)) + .start_eng3(Ratio::new::(95.)) + .start_eng4(Ratio::new::(95.)) + .on_the_ground() + .set_cold_dark_inputs() + .run_one_tick(); + + test_bed = test_bed + .set_ground_speed(Velocity::new::(15.)) + .set_rudder_input(Ratio::new::(1.)) + .run_waiting_for(Duration::from_secs(15)); + + // Expecting around 6 degrees, so ratio with 0.5 margin should be 5.5/75 + assert!(test_bed.get_nose_steering_ratio().get::() >= 5.5 / 75.); + + test_bed = test_bed + .set_ground_speed(Velocity::new::(155.)) + .run_waiting_for(Duration::from_secs(5)); + + // Expecting around 2 degrees, so ratio with 0.5 margin should be 2.5/75 + assert!(test_bed.get_nose_steering_ratio().get::() <= 2.5 / 75.); + } + + #[test] + fn landing_mode_limits_nose_steering_to_0_degrees_after_150_knot() { + let mut test_bed = test_bed_on_ground_with() + .start_eng1(Ratio::new::(95.)) + .start_eng2(Ratio::new::(95.)) + .start_eng3(Ratio::new::(95.)) + .start_eng4(Ratio::new::(95.)) + .in_flight() + .set_cold_dark_inputs() + .run_one_tick(); + + test_bed = test_bed + .set_ground_speed(Velocity::new::(160.)) + .on_the_ground() + .set_rudder_input(Ratio::new::(1.)) + .run_waiting_for(Duration::from_secs(15)); + + // Expecting 0 degrees, so ratio with 0.5 margin should be 0.5/75 + assert!(test_bed.get_nose_steering_ratio().get::() < 0.5 / 75.); + + // At lower speed in landing mode should get 6 degrees range + test_bed = test_bed + .set_ground_speed(Velocity::new::(100.)) + .run_waiting_for(Duration::from_secs(5)); + + // Expecting around 6 degrees, so ratio with .5 margin should be 5./75 + assert!(test_bed.get_nose_steering_ratio().get::() > 5.5 / 75.); + } } } diff --git a/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/body_wheel_steering.rs b/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/body_wheel_steering.rs index 2e61213f7ae..e859c388ecb 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/body_wheel_steering.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/body_wheel_steering.rs @@ -5,13 +5,17 @@ use systems_wasm::aspects::{ExecuteOn, MsfsAspectBuilder}; use systems_wasm::Variable; pub(super) fn body_wheel_steering(builder: &mut MsfsAspectBuilder) -> Result<(), Box> { - // Converting nose angle to rear steering angle: there's a 20° deadband before activating rear steering - // We model this for now with a cubic function until system is implemented for rear steering builder.map( ExecuteOn::PostTick, - Variable::named("NOSE_WHEEL_POSITION"), - |value| (4. * (value - 0.5).powi(3) + 0.5).clamp(0., 1.), - Variable::named("BODY_WHEEL_STEERING_POSITION"), + Variable::aspect("LEFT_BODY_STEERING_POSITION_RATIO"), + |value| -value / 2. + 0.5, + Variable::named("LEFT_BODY_WHEEL_STEERING_POSITION"), + ); + builder.map( + ExecuteOn::PostTick, + Variable::aspect("RIGHT_BODY_STEERING_POSITION_RATIO"), + |value| -value / 2. + 0.5, + Variable::named("RIGHT_BODY_WHEEL_STEERING_POSITION"), ); // Rear steering wheel animations @@ -20,7 +24,7 @@ pub(super) fn body_wheel_steering(builder: &mut MsfsAspectBuilder) -> Result<(), builder.map_many( ExecuteOn::PostTick, vec![ - Variable::named("BODY_WHEEL_STEERING_POSITION"), + Variable::named("LEFT_BODY_WHEEL_STEERING_POSITION"), Variable::aircraft("LEFT WHEEL ROTATION ANGLE", "degree", 0), ], |values| { @@ -31,7 +35,7 @@ pub(super) fn body_wheel_steering(builder: &mut MsfsAspectBuilder) -> Result<(), builder.map_many( ExecuteOn::PostTick, vec![ - Variable::named("BODY_WHEEL_STEERING_POSITION"), + Variable::named("LEFT_BODY_WHEEL_STEERING_POSITION"), Variable::aircraft("LEFT WHEEL ROTATION ANGLE", "degree", 0), ], |values| { @@ -44,7 +48,7 @@ pub(super) fn body_wheel_steering(builder: &mut MsfsAspectBuilder) -> Result<(), builder.map_many( ExecuteOn::PostTick, vec![ - Variable::named("BODY_WHEEL_STEERING_POSITION"), + Variable::named("RIGHT_BODY_WHEEL_STEERING_POSITION"), Variable::aircraft("RIGHT WHEEL ROTATION ANGLE", "degree", 0), ], |values| { @@ -55,7 +59,7 @@ pub(super) fn body_wheel_steering(builder: &mut MsfsAspectBuilder) -> Result<(), builder.map_many( ExecuteOn::PostTick, vec![ - Variable::named("BODY_WHEEL_STEERING_POSITION"), + Variable::named("RIGHT_BODY_WHEEL_STEERING_POSITION"), Variable::aircraft("RIGHT WHEEL ROTATION ANGLE", "degree", 0), ], |values| { diff --git a/fbw-common/src/wasm/systems/systems/src/hydraulic/linear_actuator.rs b/fbw-common/src/wasm/systems/systems/src/hydraulic/linear_actuator.rs index 334158c143b..88ac4782ba8 100644 --- a/fbw-common/src/wasm/systems/systems/src/hydraulic/linear_actuator.rs +++ b/fbw-common/src/wasm/systems/systems/src/hydraulic/linear_actuator.rs @@ -817,8 +817,12 @@ impl CoreHydraulicForce { speed: Velocity, ) -> Force { if self.is_dev_tuning_active { - self.pid_controller - .set_gains(self.test_p_gain, self.test_i_gain, self.test_force_gain); + self.pid_controller.set_gains( + self.test_p_gain, + self.test_i_gain, + 0., + self.test_force_gain, + ); } let open_loop_flow_target = self.open_loop_flow(required_position, position_normalized); diff --git a/fbw-common/src/wasm/systems/systems/src/hydraulic/nose_steering.rs b/fbw-common/src/wasm/systems/systems/src/hydraulic/nose_steering.rs index d407db92c02..2aeb967fcbf 100644 --- a/fbw-common/src/wasm/systems/systems/src/hydraulic/nose_steering.rs +++ b/fbw-common/src/wasm/systems/systems/src/hydraulic/nose_steering.rs @@ -1,4 +1,5 @@ use crate::hydraulic::linear_actuator::Actuator; +use crate::shared::Clamp; use crate::shared::{interpolation, low_pass_filter::LowPassFilter, SectionPressure}; use crate::simulation::{ InitContext, SimulationElement, SimulatorWriter, UpdateContext, VariableIdentifier, Write, @@ -111,6 +112,8 @@ pub struct SteeringActuator { actuator_area: Area, reference_pressure_for_max_speed: Pressure, + + is_steered_by_tug: bool, } impl SteeringActuator { const MIN_PRESSURE_ALLOWING_STEERING_PSI: f64 = 300.; @@ -121,18 +124,20 @@ impl SteeringActuator { // Formula is speed_coefficient = POSITION_ERROR_TO_MAX_SPEED_GAIN * position_error^2 // Then max speed will be max_speed = nominal_speed * speed_coefficient // Note this is open loop: it will overshoot or undershoot depending on this factor - const POSITION_ERROR_TO_MAX_SPEED_GAIN: f64 = 0.06; + const POSITION_ERROR_TO_MAX_SPEED_GAIN: f64 = 0.18; pub fn new( context: &mut InitContext, + wheel_id: &str, max_half_angle: Angle, nominal_speed: AngularVelocity, actuator_diameter: Length, angular_to_linear_ratio: Ratio, reference_pressure_for_max_speed: Pressure, + is_steered_by_tug: bool, ) -> Self { Self { - position_id: context.get_identifier("NOSE_WHEEL_POSITION_RATIO".to_owned()), + position_id: context.get_identifier(format!("{}_POSITION_RATIO", wheel_id)), current_speed: LowPassFilter::::new( Self::CURRENT_SPEED_FILTER_TIMECONST, @@ -152,6 +157,8 @@ impl SteeringActuator { * (actuator_diameter / 2.), reference_pressure_for_max_speed, + + is_steered_by_tug, } } @@ -163,11 +170,10 @@ impl SteeringActuator { pushback_tug: &impl Pushback, bypass_pin: &BypassPin, ) { - if !bypass_pin.is_nose_wheel_steering_pin_inserted() { + if !bypass_pin.is_nose_wheel_steering_pin_inserted() || !self.is_steered_by_tug { let limited_requested_angle = steering_controller .requested_position() - .min(self.max_half_angle) - .max(-self.max_half_angle); + .clamp(-self.max_half_angle, self.max_half_angle); self.update_current_speed(context, section_pressure, limited_requested_angle); @@ -200,7 +206,7 @@ impl SteeringActuator { let current_pressure = section_pressure.pressure_downstream_priority_valve(); let max_speed_for_current_hydraulics_pressure = - self.max_speed_for_current_hydraulics_pressure(current_pressure); + self.max_speed_for_current_hydraulics_pressure(context, current_pressure); let max_speed_closing_to_requested_position = self.max_speed_for_position_error(requested_angle); @@ -221,13 +227,15 @@ impl SteeringActuator { fn max_speed_for_current_hydraulics_pressure( &self, + context: &UpdateContext, current_pressure: Pressure, ) -> AngularVelocity { (if current_pressure.get::() > Self::MIN_PRESSURE_ALLOWING_STEERING_PSI { self.nominal_speed * current_pressure.get::().sqrt() / self.reference_pressure_for_max_speed.get::().sqrt() } else { - AngularVelocity::default() + (0.04 * context.ground_speed().get::().abs().sqrt()).clamp(0., 1.) + * self.nominal_speed }) .min(self.nominal_speed) } @@ -296,7 +304,7 @@ mod tests { use super::*; - use crate::simulation::test::{ReadByName, SimulationTestBed, TestBed}; + use crate::simulation::test::{ReadByName, SimulationTestBed, TestBed, WriteByName}; use crate::simulation::{Aircraft, SimulationElement, SimulationElementVisitor}; use std::time::Duration; use uom::si::{angle::degree, pressure::psi}; @@ -586,7 +594,7 @@ mod tests { test_bed.command(|a| a.set_pressure(Pressure::new::(3000.))); test_bed.command(|a| a.command_steer_angle(Angle::new::(20.))); - test_bed.run_multiple_frames(Duration::from_secs(3)); + test_bed.run_multiple_frames(Duration::from_secs(5)); assert!(is_equal_angle( test_bed.query(|a| a.steering_actuator.position_feedback()), @@ -674,14 +682,52 @@ mod tests { )); } + #[test] + fn steering_returns_neutral_if_ground_speed_but_no_hydraulics() { + let mut test_bed = SimulationTestBed::new(TestAircraft::new); + + test_bed.command(|a| a.set_pressure(Pressure::new::(3000.))); + test_bed.command(|a| a.command_steer_angle(Angle::new::(20.))); + + test_bed.run_multiple_frames(Duration::from_secs(5)); + + assert!(is_equal_angle( + test_bed.query(|a| a.steering_actuator.position_feedback()), + Angle::new::(20.) + )); + + test_bed.command(|a| a.set_pressure(Pressure::new::(0.))); + test_bed.command(|a| a.command_steer_angle(Angle::new::(0.))); + + test_bed.run_multiple_frames(Duration::from_secs(5)); + + assert!(is_equal_angle( + test_bed.query(|a| a.steering_actuator.position_feedback()), + Angle::new::(20.) + )); + + test_bed.write_by_name("GPS GROUND SPEED", 20.); + + test_bed.run_multiple_frames(Duration::from_secs(8)); + + assert!( + test_bed + .query(|a| a.steering_actuator.position_feedback()) + .abs() + < Angle::new::(0.1) + ); + } + fn steering_actuator(context: &mut InitContext) -> SteeringActuator { SteeringActuator::new( context, + "NOSE_WHEEL", Angle::new::(75.), AngularVelocity::new::(0.35), Length::new::(0.05), Ratio::new::(0.15), Pressure::new::(2000.), + true, ) } diff --git a/fbw-common/src/wasm/systems/systems/src/hydraulic/pushback.rs b/fbw-common/src/wasm/systems/systems/src/hydraulic/pushback.rs index 113a062240b..969c5eec0fd 100644 --- a/fbw-common/src/wasm/systems/systems/src/hydraulic/pushback.rs +++ b/fbw-common/src/wasm/systems/systems/src/hydraulic/pushback.rs @@ -1,7 +1,9 @@ -use uom::si::{angle::radian, f64::*}; +use uom::si::{f64::*, length::meter}; use crate::{ - shared::{low_pass_filter::LowPassFilter, DelayedFalseLogicGate}, + shared::{ + low_pass_filter::LowPassFilter, steering_angle_from_plane_yaw_rate, DelayedFalseLogicGate, + }, simulation::{ InitContext, Read, SimulationElement, SimulatorReader, UpdateContext, VariableIdentifier, }, @@ -12,9 +14,7 @@ use super::nose_steering::Pushback; pub struct PushbackTug { state_id: VariableIdentifier, - steer_angle_id: VariableIdentifier, - steering_angle_raw: Angle, steering_angle: LowPassFilter, // Type of pushback: @@ -32,14 +32,12 @@ impl PushbackTug { const STATE_NO_PUSHBACK: f64 = 3.; - const STEERING_ANGLE_FILTER_TIME_CONSTANT: Duration = Duration::from_millis(1500); + const STEERING_ANGLE_FILTER_TIME_CONSTANT: Duration = Duration::from_millis(800); pub fn new(context: &mut InitContext) -> Self { Self { state_id: context.get_identifier("PUSHBACK STATE".to_owned()), - steer_angle_id: context.get_identifier("PUSHBACK ANGLE".to_owned()), - steering_angle_raw: Angle::default(), steering_angle: LowPassFilter::new(Self::STEERING_ANGLE_FILTER_TIME_CONSTANT), state: Self::STATE_NO_PUSHBACK, @@ -49,16 +47,22 @@ impl PushbackTug { } } + fn update_pushback_angle(&mut self, context: &UpdateContext) { + if self.is_pushing() { + let new_angle = if context.local_velocity().to_ms_vector()[2].abs() < 0.05 { + self.steering_angle.output() + } else { + steering_angle_from_plane_yaw_rate(context, Length::new::(25.)) + }; + self.steering_angle.update(context.delta(), new_angle); + } + } + pub fn update(&mut self, context: &UpdateContext) { self.nose_wheel_steering_pin_inserted .update(context, self.is_pushing()); - if self.is_pushing() { - self.steering_angle - .update(context.delta(), self.steering_angle_raw); - } else { - self.steering_angle.reset(Angle::default()); - } + self.update_pushback_angle(context); } fn is_pushing(&self) -> bool { @@ -77,7 +81,5 @@ impl Pushback for PushbackTug { impl SimulationElement for PushbackTug { fn read(&mut self, reader: &mut SimulatorReader) { self.state = reader.read(&self.state_id); - - self.steering_angle_raw = Angle::new::(reader.read(&self.steer_angle_id)); } } diff --git a/fbw-common/src/wasm/systems/systems/src/pneumatic/mod.rs b/fbw-common/src/wasm/systems/systems/src/pneumatic/mod.rs index f64b7aaddaf..69f80cadbb5 100644 --- a/fbw-common/src/wasm/systems/systems/src/pneumatic/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/pneumatic/mod.rs @@ -1041,6 +1041,7 @@ mod tests { 0., Velocity::new::(0.), Velocity::new::(0.), + Velocity::new::(0.), altitude, altitude, InternationalStandardAtmosphere::temperature_at_altitude(altitude), diff --git a/fbw-common/src/wasm/systems/systems/src/pneumatic/valve.rs b/fbw-common/src/wasm/systems/systems/src/pneumatic/valve.rs index e8da5924b86..d63bb1d2e88 100644 --- a/fbw-common/src/wasm/systems/systems/src/pneumatic/valve.rs +++ b/fbw-common/src/wasm/systems/systems/src/pneumatic/valve.rs @@ -674,6 +674,7 @@ mod tests { 0., Velocity::new::(0.), Velocity::new::(0.), + Velocity::new::(0.), altitude, altitude, InternationalStandardAtmosphere::temperature_at_altitude(altitude), diff --git a/fbw-common/src/wasm/systems/systems/src/shared/mod.rs b/fbw-common/src/wasm/systems/systems/src/shared/mod.rs index 4c54fee5e49..8a7a3c860ae 100644 --- a/fbw-common/src/wasm/systems/systems/src/shared/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/shared/mod.rs @@ -11,6 +11,7 @@ use ntest::MaxDifference; use num_derive::FromPrimitive; use std::{cell::Ref, fmt::Display, time::Duration}; use uom::si::{ + angle::radian, f64::*, length::meter, mass_rate::kilogram_per_second, @@ -771,6 +772,23 @@ pub fn local_acceleration_at_plane_coordinate( centripetal_acceleration + tangential_acceleration_of_point } +/// Gives the steering angle for a wheel that would freely caster if plane is rotating on yaw axis +pub fn steering_angle_from_plane_yaw_rate( + context: &UpdateContext, + wheel_distance_to_rotation_center: Length, +) -> Angle { + if context.local_velocity().to_ms_vector()[2].abs() > 0.01 { + Angle::new::( + (wheel_distance_to_rotation_center.get::() + * context.rotation_velocity_rad_s()[1] + / context.local_velocity().to_ms_vector()[2]) + .atan(), + ) + } else { + Angle::default() + } +} + pub struct InternationalStandardAtmosphere; impl InternationalStandardAtmosphere { const TEMPERATURE_LAPSE_RATE: f64 = 0.0065; diff --git a/fbw-common/src/wasm/systems/systems/src/shared/pid.rs b/fbw-common/src/wasm/systems/systems/src/shared/pid.rs index a302ba5f38e..749b0474aef 100644 --- a/fbw-common/src/wasm/systems/systems/src/shared/pid.rs +++ b/fbw-common/src/wasm/systems/systems/src/shared/pid.rs @@ -116,9 +116,10 @@ impl PidController { self.max_output = max; } - pub fn set_gains(&mut self, kp: f64, ki: f64, output_gain: f64) { + pub fn set_gains(&mut self, kp: f64, ki: f64, kd: f64, output_gain: f64) { self.kp = kp; self.ki = ki; + self.kd = kd; self.output_gain = output_gain; } } diff --git a/fbw-common/src/wasm/systems/systems/src/simulation/update_context.rs b/fbw-common/src/wasm/systems/systems/src/simulation/update_context.rs index 20d84f7a4d4..5ae4262f25d 100644 --- a/fbw-common/src/wasm/systems/systems/src/simulation/update_context.rs +++ b/fbw-common/src/wasm/systems/systems/src/simulation/update_context.rs @@ -203,6 +203,7 @@ pub struct UpdateContext { ambient_temperature_id: VariableIdentifier, indicated_airspeed_id: VariableIdentifier, true_airspeed_id: VariableIdentifier, + ground_speed_id: VariableIdentifier, indicated_altitude_id: VariableIdentifier, pressure_altitude_id: VariableIdentifier, is_on_ground_id: VariableIdentifier, @@ -242,6 +243,7 @@ pub struct UpdateContext { is_ready: bool, indicated_airspeed: Velocity, true_airspeed: Velocity, + ground_speed: Velocity, indicated_altitude: Length, pressure_altitude: Length, ambient_temperature: ThermodynamicTemperature, @@ -282,6 +284,7 @@ pub struct UpdateContext { aircraft_preset_quick_mode: bool, } impl UpdateContext { + pub(crate) const GROUND_SPEED_KEY: &'static str = "GPS GROUND SPEED"; pub(crate) const IS_READY_KEY: &'static str = "IS_READY"; pub(crate) const AMBIENT_DENSITY_KEY: &'static str = "AMBIENT DENSITY"; pub(crate) const IN_CLOUD_KEY: &'static str = "AMBIENT IN CLOUD"; @@ -339,6 +342,7 @@ impl UpdateContext { simulation_time: f64, indicated_airspeed: Velocity, true_airspeed: Velocity, + ground_speed: Velocity, indicated_altitude: Length, pressure_altitude: Length, ambient_temperature: ThermodynamicTemperature, @@ -357,6 +361,7 @@ impl UpdateContext { .get_identifier(Self::AMBIENT_TEMPERATURE_KEY.to_owned()), indicated_airspeed_id: context.get_identifier(Self::INDICATED_AIRSPEED_KEY.to_owned()), true_airspeed_id: context.get_identifier(Self::TRUE_AIRSPEED_KEY.to_owned()), + ground_speed_id: context.get_identifier(Self::GROUND_SPEED_KEY.to_owned()), indicated_altitude_id: context.get_identifier(Self::INDICATED_ALTITUDE_KEY.to_owned()), pressure_altitude_id: context.get_identifier(Self::PRESSURE_ALTITUDE_KEY.to_owned()), is_on_ground_id: context.get_identifier(Self::IS_ON_GROUND_KEY.to_owned()), @@ -402,6 +407,7 @@ impl UpdateContext { is_ready: true, indicated_airspeed, true_airspeed, + ground_speed, indicated_altitude, pressure_altitude, ambient_temperature, @@ -461,6 +467,7 @@ impl UpdateContext { ambient_temperature_id: context.get_identifier("AMBIENT TEMPERATURE".to_owned()), indicated_airspeed_id: context.get_identifier("AIRSPEED INDICATED".to_owned()), true_airspeed_id: context.get_identifier("AIRSPEED TRUE".to_owned()), + ground_speed_id: context.get_identifier("GPS GROUND SPEED".to_owned()), indicated_altitude_id: context.get_identifier("INDICATED ALTITUDE".to_owned()), pressure_altitude_id: context.get_identifier("PRESSURE ALTITUDE".to_owned()), is_on_ground_id: context.get_identifier("SIM ON GROUND".to_owned()), @@ -504,6 +511,7 @@ impl UpdateContext { is_ready: Default::default(), indicated_airspeed: Default::default(), true_airspeed: Default::default(), + ground_speed: Default::default(), indicated_altitude: Default::default(), pressure_altitude: Default::default(), ambient_temperature: Default::default(), @@ -563,6 +571,7 @@ impl UpdateContext { self.ambient_temperature = reader.read(&self.ambient_temperature_id); self.indicated_airspeed = reader.read(&self.indicated_airspeed_id); self.true_airspeed = reader.read(&self.true_airspeed_id); + self.ground_speed = reader.read(&self.ground_speed_id); self.indicated_altitude = reader.read(&self.indicated_altitude_id); self.pressure_altitude = reader.read(&self.pressure_altitude_id); self.is_on_ground = reader.read(&self.is_on_ground_id); @@ -735,6 +744,10 @@ impl UpdateContext { self.true_airspeed } + pub fn ground_speed(&self) -> Velocity { + self.ground_speed + } + pub fn indicated_altitude(&self) -> Length { self.indicated_altitude }