diff --git a/Cargo.lock b/Cargo.lock index dbe16915a3a..aed8f4d2a42 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -337,7 +337,7 @@ checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72" dependencies = [ "proc-macro2", "quote", - "syn 2.0.13", + "syn 2.0.15", ] [[package]] @@ -391,9 +391,9 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.8" +version = "0.2.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c05aeb6a22b8f62540c194aac980f2115af067bfe15a0734d7277a768d396b31" +checksum = "c85e1d9ab2eadba7e5040d4e09cbd6d072b76a557ad64e797c2cb9d4da21d7e4" dependencies = [ "cfg-if", "libc", @@ -457,9 +457,9 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.140" +version = "0.2.141" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "99227334921fae1a979cf0bfdfcc6b3e5ce376ef57e16fb6fb3ea2ed6095f80c" +checksum = "3304a64d199bb964be99741b7a14d26972741915b3649639149b2479bb46f4b5" [[package]] name = "libloading" @@ -738,9 +738,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.55" +version = "1.0.56" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d0dd4be24fcdcfeaa12a432d588dc59bbad6cad3510c67e74a2b6b2fc950564" +checksum = "2b63bdb0cd06f1f4dedf69b254734f9b45af66e4a031e42a7480257d9898b435" dependencies = [ "unicode-ident", ] @@ -886,22 +886,22 @@ dependencies = [ [[package]] name = "serde" -version = "1.0.159" +version = "1.0.160" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c04e8343c3daeec41f58990b9d77068df31209f2af111e059e9fe9646693065" +checksum = "bb2f3770c8bce3bcda7e149193a069a0f4365bda1fa5cd88e03bca26afc1216c" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.159" +version = "1.0.160" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c614d17805b093df4b147b51339e7e44bf05ef59fba1e45d83500bcfb4d8585" +checksum = "291a097c63d8497e00160b166a967a4a79c64f3facdd01cbd7502231688d77df" dependencies = [ "proc-macro2", "quote", - "syn 2.0.13", + "syn 2.0.15", ] [[package]] @@ -956,9 +956,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.13" +version = "2.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c9da457c5285ac1f936ebd076af6dac17a61cfe7826f2076b4d015cf47bc8ec" +checksum = "a34fcf3e8b60f57e6a14301a2e916d323af98b0ea63c599441eec8558660c822" dependencies = [ "proc-macro2", "quote", @@ -1041,7 +1041,7 @@ checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f" dependencies = [ "proc-macro2", "quote", - "syn 2.0.13", + "syn 2.0.15", ] [[package]] diff --git a/fbw-a32nx/src/wasm/systems/a320_systems_wasm/src/lib.rs b/fbw-a32nx/src/wasm/systems/a320_systems_wasm/src/lib.rs index 6146c92bdf1..505297f9437 100644 --- a/fbw-a32nx/src/wasm/systems/a320_systems_wasm/src/lib.rs +++ b/fbw-a32nx/src/wasm/systems/a320_systems_wasm/src/lib.rs @@ -368,6 +368,21 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_named_variable("FSDT_GSX_NUMPASSENGERS_DEBOARDING_TOTAL")? .provides_named_variable("FSDT_GSX_BOARDING_CARGO_PERCENT")? .provides_named_variable("FSDT_GSX_DEBOARDING_CARGO_PERCENT")? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY X", + "radian per second squared", + 0, + )? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY Y", + "radian per second squared", + 0, + )? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY Z", + "radian per second squared", + 0, + )? .with_aspect(|builder| { builder.copy( Variable::aircraft("APU GENERATOR SWITCH", "Bool", 0), diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/mod.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/mod.rs index 337958382ee..aa7db69bea1 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/mod.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/fuel/mod.rs @@ -224,6 +224,10 @@ impl FuelPayload for A380Fuel { fn fore_aft_center_of_gravity(&self) -> f64 { self.fore_aft_center_of_gravity() } + + fn tank_mass(&self, t: usize) -> Mass { + self.fuel_system.tank_mass(t) + } } impl FuelCG for A380Fuel { fn center_of_gravity(&self) -> Vector3 { 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 6c8c392bc0d..33165032c52 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 @@ -54,7 +54,7 @@ use systems::{ AirbusElectricPumpId, AirbusEngineDrivenPumpId, DelayedFalseLogicGate, DelayedPulseTrueLogicGate, DelayedTrueLogicGate, ElectricalBusType, ElectricalBuses, EngineFirePushButtons, GearWheel, HydraulicColor, LandingGearHandle, LgciuInterface, - LgciuWeightOnWheels, ReservoirAirPressure, SectionPressure, + LgciuWeightOnWheels, ReservoirAirPressure, SectionPressure, SurfacesPositions, }, simulation::{ InitContext, Read, Reader, SimulationElement, SimulationElementVisitor, SimulatorReader, @@ -738,15 +738,15 @@ impl A380ElevatorFactory { /// Builds an aileron control surface body for A380-800 fn a380_elevator_body( init_drooped_down: bool, - is_outter: bool, + is_outer: bool, ) -> LinearActuatedRigidBodyOnHingeAxis { - let size = if is_outter { + let size = if is_outer { Vector3::new(9., 0.405, 2.23) } else { Vector3::new(5., 0.405, 2.49) }; - let mass = if is_outter { + let mass = if is_outer { Mass::new::(243.) } else { Mass::new::(189.) @@ -786,9 +786,9 @@ impl A380ElevatorFactory { context: &mut InitContext, init_drooped_down: bool, powered_by: Option, - is_outter: bool, + is_outer: bool, ) -> HydraulicLinearActuatorAssembly<2> { - let elevator_body = Self::a380_elevator_body(init_drooped_down, is_outter); + let elevator_body = Self::a380_elevator_body(init_drooped_down, is_outer); let elevator_actuator_outboard = Self::a380_elevator_actuator(context, &elevator_body, None); @@ -834,10 +834,10 @@ impl A380ElevatorFactory { ) } - fn new_a380_elevator_aero_model(is_outter: bool) -> AerodynamicModel { - let body = Self::a380_elevator_body(true, is_outter); + fn new_a380_elevator_aero_model(is_outer: bool) -> AerodynamicModel { + let body = Self::a380_elevator_body(true, is_outer); - let area_coeff = if is_outter { + let area_coeff = if is_outer { Ratio::new::(0.723) } else { Ratio::new::(0.822) @@ -2007,11 +2007,11 @@ impl A380Hydraulic { } pub fn left_elevator_aero_torques(&self) -> (Torque, Torque) { - self.left_elevator.aerodynamic_torques_outter_inner() + self.left_elevator.aerodynamic_torques_outer_inner() } pub fn right_elevator_aero_torques(&self) -> (Torque, Torque) { - self.right_elevator.aerodynamic_torques_outter_inner() + self.right_elevator.aerodynamic_torques_outer_inner() } pub fn up_down_rudder_aero_torques(&self) -> (Torque, Torque) { @@ -2802,6 +2802,31 @@ impl A380Hydraulic { &self.gear_system } } +impl SurfacesPositions for A380Hydraulic { + fn left_ailerons_positions(&self) -> &[f64] { + self.left_aileron.positions() + } + + fn right_ailerons_positions(&self) -> &[f64] { + self.right_aileron.positions() + } + + fn left_spoilers_positions(&self) -> &[f64] { + self.left_spoilers.positions() + } + + fn right_spoilers_positions(&self) -> &[f64] { + self.right_spoilers.positions() + } + + fn left_flaps_position(&self) -> f64 { + self.flap_system.left_position() + } + + fn right_flaps_position(&self) -> f64 { + self.flap_system.right_position() + } +} impl SimulationElement for A380Hydraulic { fn accept(&mut self, visitor: &mut T) { self.engine_driven_pump_1a.accept(visitor); @@ -6021,7 +6046,7 @@ struct AileronAssembly { position_mid_id: VariableIdentifier, position_in_id: VariableIdentifier, - positions: [Ratio; 3], + positions: [f64; 3], aerodynamic_models: [AerodynamicModel; 3], } impl AileronAssembly { @@ -6031,7 +6056,7 @@ impl AileronAssembly { outward_hydraulic_assembly: HydraulicLinearActuatorAssembly<2>, middle_hydraulic_assembly: HydraulicLinearActuatorAssembly<2>, inward_hydraulic_assembly: HydraulicLinearActuatorAssembly<2>, - aerodynamic_model_outter: AerodynamicModel, + aerodynamic_model_outer: AerodynamicModel, aerodynamic_model_middle: AerodynamicModel, aerodynamic_model_inner: AerodynamicModel, ) -> Self { @@ -6065,9 +6090,9 @@ impl AileronAssembly { context.get_identifier("HYD_AIL_RIGHT_INWARD_DEFLECTION".to_owned()) } }, - positions: [Ratio::new::(0.); 3], + positions: [0.; 3], aerodynamic_models: [ - aerodynamic_model_outter, + aerodynamic_model_outer, aerodynamic_model_middle, aerodynamic_model_inner, ], @@ -6102,9 +6127,15 @@ impl AileronAssembly { ], ); - self.positions[idx] = self.hydraulic_assemblies[idx].position_normalized(); + self.positions[idx] = self.hydraulic_assemblies[idx] + .position_normalized() + .get::(); } } + + fn positions(&self) -> &[f64; 3] { + &self.positions + } } impl SimulationElement for AileronAssembly { fn accept(&mut self, visitor: &mut T) { @@ -6114,9 +6145,9 @@ impl SimulationElement for AileronAssembly { } fn write(&self, writer: &mut SimulatorWriter) { - writer.write(&self.position_out_id, self.positions[0].get::()); - writer.write(&self.position_mid_id, self.positions[1].get::()); - writer.write(&self.position_in_id, self.positions[2].get::()); + writer.write(&self.position_out_id, self.positions[0]); + writer.write(&self.position_mid_id, self.positions[1]); + writer.write(&self.position_in_id, self.positions[2]); } } @@ -6136,7 +6167,7 @@ impl ElevatorAssembly { id: ActuatorSide, outward_hydraulic_assembly: HydraulicLinearActuatorAssembly<2>, inward_hydraulic_assembly: HydraulicLinearActuatorAssembly<2>, - aerodynamic_model_outter: AerodynamicModel, + aerodynamic_model_outer: AerodynamicModel, aerodynamic_model_inner: AerodynamicModel, ) -> Self { Self { @@ -6159,7 +6190,7 @@ impl ElevatorAssembly { }, positions: [Ratio::new::(0.); 2], - aerodynamic_models: [aerodynamic_model_outter, aerodynamic_model_inner], + aerodynamic_models: [aerodynamic_model_outer, aerodynamic_model_inner], } } @@ -6196,8 +6227,8 @@ impl ElevatorAssembly { } } - // Returns aerodynamic torques for (outter,inner) control surfaces - fn aerodynamic_torques_outter_inner(&self) -> (Torque, Torque) { + // Returns aerodynamic torques for (outer,inner) control surfaces + fn aerodynamic_torques_outer_inner(&self) -> (Torque, Torque) { ( self.hydraulic_assemblies[0].aerodynamic_torque(), self.hydraulic_assemblies[1].aerodynamic_torque(), @@ -6378,6 +6409,10 @@ impl SpoilerElement { self.position = self.hydraulic_assembly.position_normalized(); } + + fn position(&self) -> f64 { + self.position.get::() + } } impl SimulationElement for SpoilerElement { fn accept(&mut self, visitor: &mut T) { @@ -6394,6 +6429,7 @@ impl SimulationElement for SpoilerElement { struct SpoilerGroup { spoilers: [SpoilerElement; 8], hydraulic_controllers: [SpoilerController; 8], + spoiler_positions: [f64; 8], } impl SpoilerGroup { fn new(context: &mut InitContext, spoiler_side: &str, spoilers: [SpoilerElement; 8]) -> Self { @@ -6409,6 +6445,7 @@ impl SpoilerGroup { SpoilerController::new(context, spoiler_side, 7), SpoilerController::new(context, spoiler_side, 8), ], + spoiler_positions: [0.; 8], } } @@ -6458,11 +6495,26 @@ impl SpoilerGroup { &self.hydraulic_controllers[7], green_section.pressure_downstream_leak_valve(), ); + + self.spoiler_positions = [ + self.spoilers[0].position(), + self.spoilers[1].position(), + self.spoilers[2].position(), + self.spoilers[3].position(), + self.spoilers[4].position(), + self.spoilers[5].position(), + self.spoilers[6].position(), + self.spoilers[7].position(), + ]; } fn actuator(&mut self, spoiler_idx: usize) -> &mut impl Actuator { self.spoilers[spoiler_idx].actuator() } + + fn positions(&self) -> &[f64; 8] { + &self.spoiler_positions + } } impl SimulationElement for SpoilerGroup { fn accept(&mut self, visitor: &mut T) { diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/lib.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/lib.rs index a454eaceac8..cb4fde010f3 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems/src/lib.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/lib.rs @@ -12,6 +12,7 @@ mod navigation; mod payload; mod pneumatic; mod power_consumption; +mod structural_flex; use self::{ air_conditioning::{A380AirConditioning, A380PressurizationOverheadPanel}, @@ -19,6 +20,7 @@ use self::{ control_display_system::A380ControlDisplaySystem, fuel::A380Fuel, pneumatic::{A380Pneumatic, A380PneumaticOverheadPanel}, + structural_flex::A380StructuralFlex, }; use airframe::A380Airframe; use electrical::{ @@ -40,7 +42,6 @@ use systems::{ AuxiliaryPowerUnitFireOverheadPanel, AuxiliaryPowerUnitOverheadPanel, }, electrical::{Electricity, ElectricitySource, ExternalPowerSource}, - engine::engine_wing_flex::EnginesFlexiblePhysics, engine::{trent_engine::TrentEngine, EngineFireOverheadPanel}, enhanced_gpwc::EnhancedGroundProximityWarningComputer, hydraulic::brake_circuit::AutobrakePanel, @@ -52,7 +53,6 @@ use systems::{ simulation::{ Aircraft, InitContext, SimulationElement, SimulationElementVisitor, UpdateContext, }, - structural_flex::elevator_flex::FlexibleElevators, }; pub struct A380 { @@ -85,11 +85,10 @@ pub struct A380 { landing_gear: LandingGear, pneumatic: A380Pneumatic, radio_altimeters: A380RadioAltimeters, - engines_flex_physics: EnginesFlexiblePhysics<4>, - elevators_flex_physics: FlexibleElevators, cds: A380ControlDisplaySystem, egpwc: EnhancedGroundProximityWarningComputer, icing_simulation: Icing, + structural_flex: A380StructuralFlex, } impl A380 { pub fn new(context: &mut InitContext) -> A380 { @@ -132,8 +131,6 @@ impl A380 { landing_gear: LandingGear::new(context), pneumatic: A380Pneumatic::new(context), radio_altimeters: A380RadioAltimeters::new(context), - engines_flex_physics: EnginesFlexiblePhysics::new(context), - elevators_flex_physics: FlexibleElevators::new(context), cds: A380ControlDisplaySystem::new(context), egpwc: EnhancedGroundProximityWarningComputer::new( context, @@ -152,6 +149,7 @@ impl A380 { ), icing_simulation: Icing::new(context), + structural_flex: A380StructuralFlex::new(context), } } } @@ -278,14 +276,19 @@ impl Aircraft for A380 { [self.lgcius.lgciu1(), self.lgcius.lgciu2()], ); - self.engines_flex_physics.update(context); - self.elevators_flex_physics.update( + self.cds.update(); + + self.egpwc.update(&self.adirs, self.lgcius.lgciu1()); + + self.structural_flex.update( context, [ self.hydraulic.left_elevator_aero_torques(), self.hydraulic.right_elevator_aero_torques(), ], self.hydraulic.up_down_rudder_aero_torques(), + &self.hydraulic, + &self.fuel, ); self.cds.update(); @@ -325,11 +328,10 @@ impl SimulationElement for A380 { self.hydraulic_overhead.accept(visitor); self.landing_gear.accept(visitor); self.pneumatic.accept(visitor); - self.elevators_flex_physics.accept(visitor); - self.engines_flex_physics.accept(visitor); self.cds.accept(visitor); self.egpwc.accept(visitor); self.icing_simulation.accept(visitor); + self.structural_flex.accept(visitor); visitor.visit(self); } diff --git a/fbw-a380x/src/wasm/systems/a380_systems/src/structural_flex.rs b/fbw-a380x/src/wasm/systems/a380_systems/src/structural_flex.rs new file mode 100644 index 00000000000..838ae2055bb --- /dev/null +++ b/fbw-a380x/src/wasm/systems/a380_systems/src/structural_flex.rs @@ -0,0 +1,1253 @@ +use systems::{ + fuel::FuelPayload, + shared::SurfacesPositions, + simulation::{ + InitContext, SimulationElement, SimulationElementVisitor, SimulatorWriter, UpdateContext, + VariableIdentifier, Write, + }, + structural_flex::elevator_flex::FlexibleElevators, + structural_flex::engine_wobble::EnginesFlexiblePhysics, + structural_flex::wing_flex::{ + FlexPhysicsNG, WingAnimationMapper, WingFuelNodeMapper, WingLift, WingRootAcceleration, + }, + structural_flex::SurfaceVibrationGenerator, +}; + +use crate::fuel::A380FuelTankType; + +use uom::si::{f64::*, force::newton, mass::kilogram, ratio::ratio}; + +use nalgebra::{Vector3, Vector5}; + +pub struct A380StructuralFlex { + ground_weight_ratio_id: VariableIdentifier, + + engines_flex_physics: EnginesFlexiblePhysics<4>, + elevators_flex_physics: FlexibleElevators, + wing_flex: WingFlexA380, + + surface_vibrations: SurfaceVibrationGenerator, +} +impl A380StructuralFlex { + pub fn new(context: &mut InitContext) -> Self { + Self { + ground_weight_ratio_id: context + .get_identifier("GROUND_WEIGHT_ON_WHEELS_RATIO".to_owned()), + + engines_flex_physics: EnginesFlexiblePhysics::new(context), + elevators_flex_physics: FlexibleElevators::new(context), + wing_flex: WingFlexA380::new(context), + + surface_vibrations: SurfaceVibrationGenerator::default(), + } + } + + pub fn update( + &mut self, + context: &UpdateContext, + outer_inner_elevator_aero_torques: [(Torque, Torque); 2], + up_down_rudder_aero_torques: (Torque, Torque), + surfaces_positions: &impl SurfacesPositions, + fuel_mass: &impl FuelPayload, + ) { + self.elevators_flex_physics.update( + context, + outer_inner_elevator_aero_torques, + up_down_rudder_aero_torques, + self.surface_vibrations.surface_vibration_acceleration(), + ); + + self.wing_flex.update( + context, + self.surface_vibrations.surface_vibration_acceleration(), + surfaces_positions, + fuel_mass, + ); + + self.engines_flex_physics + .update(context, self.wing_flex.accelerations_at_engines_pylons()); + + self.surface_vibrations + .update(context, self.wing_flex.ground_weight_ratio()); + } +} +impl SimulationElement for A380StructuralFlex { + fn accept(&mut self, visitor: &mut T) { + self.elevators_flex_physics.accept(visitor); + self.engines_flex_physics.accept(visitor); + self.wing_flex.accept(visitor); + + visitor.visit(self); + } + + fn write(&self, writer: &mut SimulatorWriter) { + writer.write( + &self.ground_weight_ratio_id, + self.wing_flex.ground_weight_ratio().get::(), + ); + } +} + +struct A380WingLiftModifier { + lateral_offset: Ratio, + + left_wing_lift: Force, + right_wing_lift: Force, + + standard_lift_spread: Vector5, + lift_left_table_newton: Vector5, + lift_right_table_newton: Vector5, +} +impl A380WingLiftModifier { + const LATERAL_OFFSET_GAIN: f64 = 0.25; + + // Ratio of the total lift on each wing section. + // Sum shall be 1. + const NOMINAL_WING_LIFT_SPREAD_RATIOS: [f64; 5] = [0., 0.42, 0.40, 0.15, 0.03]; + + // GAIN to determine how much a surface spoils lift when deployed. 0.3 means a fully deployed surface reduce lift by 30% + const SPOILER_SURFACES_SPOIL_GAIN: f64 = 0.4; + const AILERON_SURFACES_SPOIL_GAIN: f64 = 0.2; + const FLAPS_SURFACES_SPOIL_GAIN: f64 = 0.3; + + fn update(&mut self, total_lift: Force, surfaces_positions: &impl SurfacesPositions) { + self.compute_lift_modifiers(total_lift, surfaces_positions); + } + + fn compute_lift_modifiers( + &mut self, + total_lift: Force, + surfaces_positions: &impl SurfacesPositions, + ) { + let wing_base_left_spoilers = surfaces_positions.left_spoilers_positions()[0..=1] + .iter() + .sum::() + / 2.; + let wing_mid_left_spoilers = surfaces_positions.left_spoilers_positions()[2..=7] + .iter() + .sum::() + / 6.; + + let wing_base_right_spoilers = surfaces_positions.left_spoilers_positions()[0..=1] + .iter() + .sum::() + / 2.; + let wing_mid_right_spoilers = surfaces_positions.left_spoilers_positions()[2..=7] + .iter() + .sum::() + / 6.; + + // Aileron position is converted from [0 1] to [-1 1] then we take the mean value + // ((position1 - 0.5) * 2. + (position2 - 0.5) * 2.) / 2. <=> position1+position2 - 1 + let left_ailerons_mid = surfaces_positions.left_ailerons_positions()[0..=1] + .iter() + .sum::() + - 1.; + let right_ailerons_mid = surfaces_positions.right_ailerons_positions()[0..=1] + .iter() + .sum::() + - 1.; + + let left_ailerons_tip = 2. * surfaces_positions.left_ailerons_positions()[2] - 1.; + let right_ailerons_tip = 2. * surfaces_positions.right_ailerons_positions()[2] - 1.; + + self.lateral_offset = Ratio::new::( + ((wing_base_right_spoilers - wing_base_left_spoilers) + + (wing_mid_right_spoilers - wing_mid_left_spoilers) + + (right_ailerons_mid - left_ailerons_mid) + + (right_ailerons_tip - left_ailerons_tip)) + / 4., + ); + + self.lateral_offset *= Self::LATERAL_OFFSET_GAIN; + + self.left_wing_lift = 0.5 * (total_lift + self.lateral_offset() * total_lift); + self.right_wing_lift = total_lift - self.left_wing_lift; + + let left_flap_lift_factor = surfaces_positions.left_flaps_position(); + let right_flap_lift_factor = surfaces_positions.right_flaps_position(); + + // Lift factor is 1 - spoil factor. We consider positive position for a surface is spoiling lift + // Spoiler panel deployed will be 1. Aileron Up will be 1. Aileron down will be -1 thus (1 - -1) = 2 adds lift + let left_wing_lift_factor = Vector5::from([ + 0., + (1. - wing_base_left_spoilers) * Self::SPOILER_SURFACES_SPOIL_GAIN + + left_flap_lift_factor * Self::FLAPS_SURFACES_SPOIL_GAIN, + (1. - wing_mid_left_spoilers) * Self::SPOILER_SURFACES_SPOIL_GAIN + + left_flap_lift_factor * Self::FLAPS_SURFACES_SPOIL_GAIN, + (1. - left_ailerons_mid) * Self::AILERON_SURFACES_SPOIL_GAIN, + (1. - left_ailerons_tip) * Self::AILERON_SURFACES_SPOIL_GAIN, + ]); + let right_wing_lift_factor = Vector5::from([ + 0., + (1. - wing_base_right_spoilers) * Self::SPOILER_SURFACES_SPOIL_GAIN + + right_flap_lift_factor * Self::FLAPS_SURFACES_SPOIL_GAIN, + (1. - wing_mid_right_spoilers) * Self::SPOILER_SURFACES_SPOIL_GAIN + + right_flap_lift_factor * Self::FLAPS_SURFACES_SPOIL_GAIN, + (1. - right_ailerons_mid) * Self::AILERON_SURFACES_SPOIL_GAIN, + (1. - right_ailerons_tip) * Self::AILERON_SURFACES_SPOIL_GAIN, + ]); + + let raw_left_total_factor = left_wing_lift_factor.component_mul(&self.standard_lift_spread); + let raw_right_total_factor = + right_wing_lift_factor.component_mul(&self.standard_lift_spread); + + let left_lift_factor_normalized = raw_left_total_factor / raw_left_total_factor.sum(); + let right_lift_factor_normalized = raw_right_total_factor / raw_right_total_factor.sum(); + + self.lift_left_table_newton = + left_lift_factor_normalized * self.left_wing_lift.get::(); + + self.lift_right_table_newton = + right_lift_factor_normalized * self.right_wing_lift.get::(); + } + + fn lateral_offset(&self) -> Ratio { + self.lateral_offset + } + + fn per_node_lift_left_wing_newton(&self) -> Vector5 { + self.lift_left_table_newton + } + + fn per_node_lift_right_wing_newton(&self) -> Vector5 { + self.lift_right_table_newton + } +} +impl Default for A380WingLiftModifier { + fn default() -> Self { + assert!(Vector5::from(Self::NOMINAL_WING_LIFT_SPREAD_RATIOS).sum() == 1.); + + Self { + lateral_offset: Ratio::default(), + + left_wing_lift: Force::default(), + right_wing_lift: Force::default(), + + standard_lift_spread: Vector5::from(Self::NOMINAL_WING_LIFT_SPREAD_RATIOS), + lift_left_table_newton: Vector5::default(), + lift_right_table_newton: Vector5::default(), + } + } +} + +const WING_FLEX_NODE_NUMBER: usize = 5; +const WING_FLEX_LINK_NUMBER: usize = WING_FLEX_NODE_NUMBER - 1; +const FUEL_TANKS_NUMBER: usize = 5; + +pub struct WingFlexA380 { + left_flex_inboard_id: VariableIdentifier, + left_flex_inboard_mid_id: VariableIdentifier, + left_flex_outboard_mid_id: VariableIdentifier, + left_flex_outboard_id: VariableIdentifier, + + right_flex_inboard_id: VariableIdentifier, + right_flex_inboard_mid_id: VariableIdentifier, + right_flex_outboard_mid_id: VariableIdentifier, + right_flex_outboard_id: VariableIdentifier, + + wing_lift: WingLift, + wing_lift_dynamic: A380WingLiftModifier, + + left_wing_fuel_mass: [Mass; FUEL_TANKS_NUMBER], + right_wing_fuel_mass: [Mass; FUEL_TANKS_NUMBER], + + fuel_mapper: WingFuelNodeMapper, + animation_mapper: WingAnimationMapper, + + flex_physics: [FlexPhysicsNG; 2], + + left_right_wing_root_position: [WingRootAcceleration; 2], +} +impl WingFlexA380 { + const FLEX_COEFFICIENTS: [f64; WING_FLEX_LINK_NUMBER] = + [20000000., 8000000., 5000000., 500000.]; + const DAMPING_COEFFICIENTS: [f64; WING_FLEX_LINK_NUMBER] = [800000., 400000., 150000., 6000.]; + + const EMPTY_MASS_KG: [f64; WING_FLEX_NODE_NUMBER] = [0., 25000., 20000., 5000., 400.]; + + const FUEL_MAPPING: [usize; FUEL_TANKS_NUMBER] = [1, 1, 2, 2, 3]; + + const WING_NODES_X_COORDINATES: [f64; WING_FLEX_NODE_NUMBER] = [0., 11.5, 22.05, 29., 36.85]; + + pub fn new(context: &mut InitContext) -> Self { + let empty_mass = Self::EMPTY_MASS_KG.map(Mass::new::); + + Self { + left_flex_inboard_id: context.get_identifier("WING_FLEX_LEFT_INBOARD".to_owned()), + left_flex_inboard_mid_id: context + .get_identifier("WING_FLEX_LEFT_INBOARD_MID".to_owned()), + left_flex_outboard_mid_id: context + .get_identifier("WING_FLEX_LEFT_OUTBOARD_MID".to_owned()), + left_flex_outboard_id: context.get_identifier("WING_FLEX_LEFT_OUTBOARD".to_owned()), + + right_flex_inboard_id: context.get_identifier("WING_FLEX_RIGHT_INBOARD".to_owned()), + right_flex_inboard_mid_id: context + .get_identifier("WING_FLEX_RIGHT_INBOARD_MID".to_owned()), + right_flex_outboard_mid_id: context + .get_identifier("WING_FLEX_RIGHT_OUTBOARD_MID".to_owned()), + right_flex_outboard_id: context.get_identifier("WING_FLEX_RIGHT_OUTBOARD".to_owned()), + + wing_lift: WingLift::new(context), + wing_lift_dynamic: A380WingLiftModifier::default(), + + left_wing_fuel_mass: [Mass::default(); FUEL_TANKS_NUMBER], + right_wing_fuel_mass: [Mass::default(); FUEL_TANKS_NUMBER], + + fuel_mapper: WingFuelNodeMapper::new(Self::FUEL_MAPPING), + animation_mapper: WingAnimationMapper::new(Self::WING_NODES_X_COORDINATES), + + flex_physics: [1, 2].map(|_| { + FlexPhysicsNG::new( + context, + empty_mass, + Self::FLEX_COEFFICIENTS, + Self::DAMPING_COEFFICIENTS, + ) + }), + + left_right_wing_root_position: [-1., 1.] + .map(|xneg| WingRootAcceleration::new(Vector3::new(xneg * 3.33668, -0.273, 6.903))), + } + } + + fn update_fuel_masses(&mut self, fuel_mass: &impl FuelPayload) { + self.left_wing_fuel_mass = [ + fuel_mass.tank_mass(A380FuelTankType::LeftInner as usize), + fuel_mass.tank_mass(A380FuelTankType::FeedTwo as usize), + fuel_mass.tank_mass(A380FuelTankType::LeftMid as usize), + fuel_mass.tank_mass(A380FuelTankType::FeedOne as usize), + fuel_mass.tank_mass(A380FuelTankType::LeftOuter as usize), + ]; + self.right_wing_fuel_mass = [ + fuel_mass.tank_mass(A380FuelTankType::RightInner as usize), + fuel_mass.tank_mass(A380FuelTankType::FeedThree as usize), + fuel_mass.tank_mass(A380FuelTankType::RightMid as usize), + fuel_mass.tank_mass(A380FuelTankType::FeedFour as usize), + fuel_mass.tank_mass(A380FuelTankType::RightOuter as usize), + ]; + } + + pub fn update( + &mut self, + context: &UpdateContext, + surface_vibration_acceleration: Acceleration, + surfaces_positions: &impl SurfacesPositions, + fuel_mass: &impl FuelPayload, + ) { + self.wing_lift.update(context); + self.wing_lift_dynamic + .update(self.wing_lift.total_plane_lift(), surfaces_positions); + + self.left_right_wing_root_position[0].update(context); + self.left_right_wing_root_position[1].update(context); + + self.update_fuel_masses(fuel_mass); + + self.flex_physics[0].update( + context, + self.wing_lift_dynamic + .per_node_lift_left_wing_newton() + .as_slice(), + self.fuel_mapper.fuel_masses(self.left_wing_fuel_mass), + surface_vibration_acceleration + self.left_right_wing_root_position[0].acceleration(), + ); + + self.flex_physics[1].update( + context, + self.wing_lift_dynamic + .per_node_lift_right_wing_newton() + .as_slice(), + self.fuel_mapper.fuel_masses(self.right_wing_fuel_mass), + surface_vibration_acceleration + self.left_right_wing_root_position[1].acceleration(), + ); + } + + pub fn ground_weight_ratio(&self) -> Ratio { + self.wing_lift.ground_weight_ratio() + } + + // Accelerations (vertical) of engines pylons from eng1 to eng4 + pub fn accelerations_at_engines_pylons(&self) -> [Acceleration; 4] { + [(0, 2), (0, 1), (1, 1), (1, 2)].map(|(phys_idx, node_idx)| { + self.flex_physics[phys_idx].acceleration_at_node_idx(node_idx) + }) + } + + #[cfg(test)] + fn left_node_position(&self, node_id: usize) -> f64 { + self.flex_physics[0].nodes_height_meters()[node_id] + } + #[cfg(test)] + fn right_node_position(&self, node_id: usize) -> f64 { + self.flex_physics[1].nodes_height_meters()[node_id] + } +} +impl SimulationElement for WingFlexA380 { + fn accept(&mut self, visitor: &mut T) { + self.wing_lift.accept(visitor); + + // Calling only left wing as this is only used for dev purpose : live tuning of flex properties + self.flex_physics[0].accept(visitor); + + visitor.visit(self); + } + + fn write(&self, writer: &mut SimulatorWriter) { + let bones_angles_left = self + .animation_mapper + .animation_angles(self.flex_physics[0].nodes_height_meters()); + + writer.write(&self.left_flex_inboard_id, bones_angles_left[1]); + writer.write(&self.left_flex_inboard_mid_id, bones_angles_left[2]); + writer.write(&self.left_flex_outboard_mid_id, bones_angles_left[3]); + writer.write(&self.left_flex_outboard_id, bones_angles_left[4]); + + let bones_angles_right = self + .animation_mapper + .animation_angles(self.flex_physics[1].nodes_height_meters()); + + writer.write(&self.right_flex_inboard_id, bones_angles_right[1]); + writer.write(&self.right_flex_inboard_mid_id, bones_angles_right[2]); + writer.write(&self.right_flex_outboard_mid_id, bones_angles_right[3]); + writer.write(&self.right_flex_outboard_id, bones_angles_right[4]); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + use crate::systems::simulation::test::{ElementCtorFn, WriteByName}; + use crate::systems::simulation::test::{SimulationTestBed, TestBed}; + use crate::systems::simulation::{Aircraft, SimulationElement, SimulationElementVisitor}; + use std::time::Duration; + + use uom::si::{angle::degree, velocity::knot}; + + use ntest::assert_about_eq; + + struct TestSurfacesPositions { + left_ailerons: [f64; 3], + right_ailerons: [f64; 3], + left_spoilers: [f64; 8], + right_spoilers: [f64; 8], + left_flaps: f64, + right_flaps: f64, + } + impl TestSurfacesPositions { + fn default() -> Self { + Self { + left_ailerons: [0.5; 3], + right_ailerons: [0.5; 3], + left_spoilers: [0.; 8], + right_spoilers: [0.; 8], + left_flaps: 0., + right_flaps: 0., + } + } + } + impl SurfacesPositions for TestSurfacesPositions { + fn left_ailerons_positions(&self) -> &[f64] { + &self.left_ailerons + } + + fn right_ailerons_positions(&self) -> &[f64] { + &self.right_ailerons + } + + fn left_spoilers_positions(&self) -> &[f64] { + &self.left_spoilers + } + + fn right_spoilers_positions(&self) -> &[f64] { + &self.right_spoilers + } + + fn left_flaps_position(&self) -> f64 { + self.left_flaps + } + + fn right_flaps_position(&self) -> f64 { + self.right_flaps + } + } + + struct TestFuelPayload { + fuel_mass: [Mass; 11], + } + impl TestFuelPayload { + fn default() -> Self { + Self { + fuel_mass: [Mass::default(); 11], + } + } + + fn set_fuel(&mut self, tank_id: A380FuelTankType, mass: Mass) { + self.fuel_mass[tank_id as usize] = mass; + } + } + impl FuelPayload for TestFuelPayload { + fn fore_aft_center_of_gravity(&self) -> f64 { + 0. + } + + fn total_load(&self) -> Mass { + Mass::default() + } + + fn tank_mass(&self, t: usize) -> Mass { + self.fuel_mass[t] + } + } + + struct WingFlexTestAircraft { + wing_flex: WingFlexA380, + surfaces_position: TestSurfacesPositions, + fuel_payload: TestFuelPayload, + } + impl WingFlexTestAircraft { + fn new(context: &mut InitContext) -> Self { + Self { + wing_flex: WingFlexA380::new(context), + surfaces_position: TestSurfacesPositions::default(), + fuel_payload: TestFuelPayload::default(), + } + } + + fn set_left_flaps(&mut self, position: f64) { + self.surfaces_position.left_flaps = position; + } + + fn set_right_flaps(&mut self, position: f64) { + self.surfaces_position.right_flaps = position; + } + + fn set_left_spoilers(&mut self, positions: [f64; 8]) { + self.surfaces_position.left_spoilers = positions; + } + + fn set_right_spoilers(&mut self, positions: [f64; 8]) { + self.surfaces_position.right_spoilers = positions; + } + + fn set_left_ailerons(&mut self, positions: [f64; 3]) { + self.surfaces_position.left_ailerons = positions; + } + + fn set_right_ailerons(&mut self, positions: [f64; 3]) { + self.surfaces_position.right_ailerons = positions; + } + + fn set_fuel(&mut self, tank_id: A380FuelTankType, mass: Mass) { + self.fuel_payload.set_fuel(tank_id, mass); + } + } + impl Aircraft for WingFlexTestAircraft { + fn update_after_power_distribution(&mut self, context: &UpdateContext) { + self.wing_flex.update( + context, + Acceleration::default(), + &self.surfaces_position, + &self.fuel_payload, + ); + + println!( + "WING HEIGHTS L/O\\R => {:.2}_{:.2}_{:.2}_{:.2}_{:.2}/O\\{:.2}_{:.2}_{:.2}_{:.2}_{:.2}", + self.wing_flex.left_node_position(4), + self.wing_flex.left_node_position(3), + self.wing_flex.left_node_position(2), + self.wing_flex.left_node_position(1), + self.wing_flex.left_node_position(0), + self.wing_flex.right_node_position(0), + self.wing_flex.right_node_position(1), + self.wing_flex.right_node_position(2), + self.wing_flex.right_node_position(3), + self.wing_flex.right_node_position(4), + ); + } + } + impl SimulationElement for WingFlexTestAircraft { + fn accept(&mut self, visitor: &mut V) { + self.wing_flex.accept(visitor); + + visitor.visit(self); + } + } + + struct WingFlexTestBed { + test_bed: SimulationTestBed, + } + impl WingFlexTestBed { + const NOMINAL_WEIGHT_KG: f64 = 400000.; + + fn new() -> Self { + Self { + test_bed: SimulationTestBed::new(WingFlexTestAircraft::new), + } + } + + fn left_wing_lift_per_node(&self) -> Vector5 { + self.query(|a| { + a.wing_flex + .wing_lift_dynamic + .per_node_lift_left_wing_newton() + }) + } + + fn right_wing_lift_per_node(&self) -> Vector5 { + self.query(|a| { + a.wing_flex + .wing_lift_dynamic + .per_node_lift_right_wing_newton() + }) + } + + fn current_total_lift(&self) -> Force { + self.query(|a| a.wing_flex.wing_lift.total_lift()) + } + + fn current_left_wing_lift(&self) -> Force { + self.query(|a| a.wing_flex.wing_lift_dynamic.left_wing_lift) + } + + fn current_right_wing_lift(&self) -> Force { + self.query(|a| a.wing_flex.wing_lift_dynamic.right_wing_lift) + } + + fn with_nominal_weight(mut self) -> Self { + self.write_by_name( + "TOTAL WEIGHT", + Mass::new::(Self::NOMINAL_WEIGHT_KG), + ); + self + } + + fn with_max_fuel(mut self) -> Self { + self.command(|a| { + a.set_fuel(A380FuelTankType::LeftInner, Mass::new::(32000.)) + }); + self.command(|a| a.set_fuel(A380FuelTankType::FeedTwo, Mass::new::(20000.))); + self.command(|a| a.set_fuel(A380FuelTankType::LeftMid, Mass::new::(28000.))); + self.command(|a| a.set_fuel(A380FuelTankType::FeedOne, Mass::new::(20000.))); + self.command(|a| a.set_fuel(A380FuelTankType::LeftOuter, Mass::new::(7200.))); + + self.command(|a| { + a.set_fuel(A380FuelTankType::RightInner, Mass::new::(32000.)) + }); + self.command(|a| { + a.set_fuel(A380FuelTankType::FeedThree, Mass::new::(20000.)) + }); + self.command(|a| a.set_fuel(A380FuelTankType::RightMid, Mass::new::(28000.))); + self.command(|a| a.set_fuel(A380FuelTankType::FeedFour, Mass::new::(20000.))); + self.command(|a| { + a.set_fuel(A380FuelTankType::RightOuter, Mass::new::(7200.)) + }); + + self + } + + fn rotate_for_takeoff(mut self) -> Self { + self.write_by_name("TOTAL WEIGHT", Mass::new::(400000.)); + self.write_by_name("AIRSPEED TRUE", Velocity::new::(150.)); + + self.write_by_name("CONTACT POINT COMPRESSION:1", 30.); + self.write_by_name("CONTACT POINT COMPRESSION:2", 30.); + + self.command(|a| a.set_left_flaps(0.5)); + self.command(|a| a.set_right_flaps(0.5)); + self + } + + fn in_1g_flight(mut self) -> Self { + self = self.neutral_ailerons(); + self = self.spoilers_retracted(); + self.write_by_name("TOTAL WEIGHT", Mass::new::(400000.)); + self.write_by_name("AIRSPEED TRUE", Velocity::new::(200.)); + self.write_by_name("CONTACT POINT COMPRESSION:1", 0.); + self.write_by_name("CONTACT POINT COMPRESSION:2", 0.); + self.write_by_name("CONTACT POINT COMPRESSION:3", 0.); + self.write_by_name("CONTACT POINT COMPRESSION:4", 0.); + self.write_by_name("CONTACT POINT COMPRESSION:5", 0.); + + self + } + + fn steady_on_ground(mut self) -> Self { + self.write_by_name("TOTAL WEIGHT", Mass::new::(400000.)); + self.write_by_name("CONTACT POINT COMPRESSION:1", 70.); + self.write_by_name("CONTACT POINT COMPRESSION:2", 70.); + self.write_by_name("CONTACT POINT COMPRESSION:3", 70.); + self.write_by_name("CONTACT POINT COMPRESSION:4", 70.); + self.write_by_name("CONTACT POINT COMPRESSION:5", 70.); + + self + } + + fn left_turn_ailerons(mut self) -> Self { + self.command(|a| a.set_right_ailerons([0.2, 0.2, 0.2])); + self.command(|a| a.set_left_ailerons([0.8, 0.8, 0.8])); + + self + } + + fn right_turn_ailerons(mut self) -> Self { + self.command(|a| a.set_left_ailerons([0.2, 0.2, 0.2])); + self.command(|a| a.set_right_ailerons([0.8, 0.8, 0.8])); + + self + } + + fn neutral_ailerons(mut self) -> Self { + self.command(|a| a.set_left_ailerons([0.5, 0.5, 0.5])); + self.command(|a| a.set_right_ailerons([0.5, 0.5, 0.5])); + + self + } + + fn spoilers_retracted(mut self) -> Self { + self.command(|a| a.set_left_spoilers([0., 0., 0., 0., 0., 0., 0., 0.])); + self.command(|a| a.set_right_spoilers([0., 0., 0., 0., 0., 0., 0., 0.])); + + self + } + + fn spoilers_left_turn(mut self) -> Self { + self.command(|a| a.set_left_spoilers([0., 0., 0., 0.5, 0.5, 0.5, 0.5, 0.5])); + self.command(|a| a.set_right_spoilers([0., 0., 0., 0., 0., 0., 0., 0.])); + + self + } + + fn spoilers_right_turn(mut self) -> Self { + self.command(|a| a.set_right_spoilers([0., 0., 0., 0.5, 0.5, 0.5, 0.5, 0.5])); + self.command(|a| a.set_left_spoilers([0., 0., 0., 0., 0., 0., 0., 0.])); + + self + } + + fn run_waiting_for(mut self, delta: Duration) -> Self { + self.test_bed.run_multiple_frames(delta); + self + } + } + impl TestBed for WingFlexTestBed { + type Aircraft = WingFlexTestAircraft; + + fn test_bed(&self) -> &SimulationTestBed { + &self.test_bed + } + + fn test_bed_mut(&mut self) -> &mut SimulationTestBed { + &mut self.test_bed + } + } + + #[test] + fn fuel_mapping_tanks_1_2_left_wing() { + let mut test_bed = SimulationTestBed::new(WingFlexTestAircraft::new); + + test_bed.run_with_delta(Duration::from_secs(1)); + + let mut node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + let mut node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + let mut node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + let mut node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + let mut node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES empty: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node2_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + + test_bed.command(|a| a.set_fuel(A380FuelTankType::LeftInner, Mass::new::(3000.))); + + test_bed.run_with_delta(Duration::from_secs(1)); + + node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES after fueling inner: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node2_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + assert!(node1_mass.get::() >= 3000. && node1_mass.get::() <= 3100.); + + test_bed.command(|a| a.set_fuel(A380FuelTankType::FeedTwo, Mass::new::(3000.))); + + test_bed.run_with_delta(Duration::from_secs(1)); + + node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + //Second tank should map to same node + + println!( + "FUELMASSES after fueling feed 2: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node2_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + assert!(node1_mass.get::() >= 6000. && node1_mass.get::() <= 6200.); + } + + #[test] + fn fuel_mapping_tanks_3_4_left_wing() { + let mut test_bed = SimulationTestBed::new(WingFlexTestAircraft::new); + + test_bed.run_with_delta(Duration::from_secs(1)); + + let mut node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + let mut node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + let mut node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + let mut node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + let mut node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES empty: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node2_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + + test_bed.command(|a| a.set_fuel(A380FuelTankType::LeftMid, Mass::new::(3000.))); + + test_bed.run_with_delta(Duration::from_secs(1)); + + node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES after fueling left mid: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + assert!(node2_mass.get::() >= 3000. && node2_mass.get::() <= 3100.); + + test_bed.command(|a| a.set_fuel(A380FuelTankType::FeedOne, Mass::new::(3000.))); + + test_bed.run_with_delta(Duration::from_secs(1)); + + node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + //Second tank should map to same node + + println!( + "FUELMASSES after fueling left feed 1: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + assert!(node2_mass.get::() >= 6000. && node1_mass.get::() <= 6200.); + } + + #[test] + fn fuel_mapping_tanks_5_left_wing() { + let mut test_bed = SimulationTestBed::new(WingFlexTestAircraft::new); + + test_bed.run_with_delta(Duration::from_secs(1)); + + let mut node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + let mut node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + let mut node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + let mut node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + let mut node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES empty: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node2_mass.get::() + + node3_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + + test_bed.command(|a| a.set_fuel(A380FuelTankType::LeftOuter, Mass::new::(3000.))); + + test_bed.run_with_delta(Duration::from_secs(1)); + + node0_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(0)); + node1_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(1)); + node2_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(2)); + node3_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(3)); + node4_mass = test_bed.query(|a| a.wing_flex.flex_physics[0].node_fuel_mass(4)); + + println!( + "FUELMASSES after fueling left outer: {:.0}/{:.0}/{:.0}/{:.0}/{:.0}", + node0_mass.get::(), + node1_mass.get::(), + node2_mass.get::(), + node3_mass.get::(), + node4_mass.get::(), + ); + + assert!( + node0_mass.get::() + + node1_mass.get::() + + node2_mass.get::() + + node4_mass.get::() + <= 0.1 + ); + assert!(node3_mass.get::() >= 3000. && node2_mass.get::() <= 3100.); + } + + #[test] + fn animation_angles_with_0_heights_gives_zero_angles() { + let test_bed = SimulationTestBed::from(ElementCtorFn(|_| { + WingAnimationMapper::<5>::new([0., 1., 2., 3., 4.]) + })); + + let anim_angles = test_bed.query_element(|e| e.animation_angles([0.0, 0.0, 0.0, 0., 0.])); + + assert!(anim_angles[0] == Angle::default()); + assert!(anim_angles[1] == Angle::default()); + assert!(anim_angles[2] == Angle::default()); + assert!(anim_angles[3] == Angle::default()); + assert!(anim_angles[4] == Angle::default()); + } + + #[test] + fn animation_angles_with_last_node_moved_1_up_gives_correct_angles() { + let test_bed = SimulationTestBed::from(ElementCtorFn(|_| { + WingAnimationMapper::<5>::new([0., 1., 2., 3., 4.]) + })); + + let anim_angles = test_bed.query_element(|e| e.animation_angles([0.0, 0.0, 0.0, 0., 1.])); + + assert!(anim_angles[0] == Angle::default()); + assert!(anim_angles[1] == Angle::default()); + assert!(anim_angles[2] == Angle::default()); + assert!(anim_angles[3] == Angle::default()); + assert!(anim_angles[4].get::() >= 44. && anim_angles[4].get::() <= 46.); + } + + #[test] + fn animation_angles_with_last_node_moved_1_down_gives_correct_angles() { + let test_bed = SimulationTestBed::from(ElementCtorFn(|_| { + WingAnimationMapper::<5>::new([0., 1., 2., 3., 4.]) + })); + + let anim_angles = test_bed.query_element(|e| e.animation_angles([0.0, 0.0, 0.0, 0., -1.])); + + assert!(anim_angles[0] == Angle::default()); + assert!(anim_angles[1] == Angle::default()); + assert!(anim_angles[2] == Angle::default()); + assert!(anim_angles[3] == Angle::default()); + assert!(anim_angles[4].get::() >= -46. && anim_angles[4].get::() <= -44.); + } + + #[test] + fn animation_angles_with_first_node_moved_1_up_gives_correct_angles() { + let test_bed = SimulationTestBed::from(ElementCtorFn(|_| { + WingAnimationMapper::<5>::new([0., 1., 2., 3., 4.]) + })); + + let anim_angles = test_bed.query_element(|e| e.animation_angles([0., 1., 1., 1., 1.])); + + assert_about_eq!( + anim_angles[0].get::(), + Angle::default().get::(), + 1.0e-3 + ); + assert!(anim_angles[1].get::() >= 44. && anim_angles[0].get::() <= 46.); + assert_about_eq!( + anim_angles[2].get::(), + Angle::default().get::(), + 1.0e-3 + ); + assert_about_eq!( + anim_angles[3].get::(), + Angle::default().get::(), + 1.0e-3 + ); + assert_about_eq!( + anim_angles[4].get::(), + Angle::default().get::(), + 1.0e-3 + ); + } + + #[test] + fn animation_angles_with_first_node_moved_1_down_gives_correct_angles() { + let test_bed = SimulationTestBed::from(ElementCtorFn(|_| { + WingAnimationMapper::<5>::new([0., 1., 2., 3., 4.]) + })); + + let anim_angles = test_bed.query_element(|e| e.animation_angles([0., -1., -1., -1., -1.])); + + assert!(anim_angles[0] == Angle::default()); + assert!(anim_angles[1].get::() <= -44. && anim_angles[1].get::() >= -46.); + assert_about_eq!( + anim_angles[2].get::(), + Angle::default().get::(), + 1.0e-3 + ); + assert_about_eq!( + anim_angles[3].get::(), + Angle::default().get::(), + 1.0e-3 + ); + assert_about_eq!( + anim_angles[4].get::(), + Angle::default().get::(), + 1.0e-3 + ); + } + + #[test] + fn steady_on_ground() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .steady_on_ground(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(3)); + + assert!(test_bed.current_total_lift().get::() / 9.8 < 1000.); + assert!(test_bed.current_total_lift().get::() / 9.8 > -1000.); + } + + #[test] + fn steady_on_ground_full_fuel() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .with_max_fuel() + .steady_on_ground(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(3)); + + assert!(test_bed.current_total_lift().get::() / 9.8 < 1000.); + assert!(test_bed.current_total_lift().get::() / 9.8 > -1000.); + } + + #[test] + fn with_some_lift_on_ground_rotation() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .rotate_for_takeoff(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + assert!( + test_bed.current_total_lift().get::() / 9.8 + < WingFlexTestBed::NOMINAL_WEIGHT_KG + ); + assert!( + test_bed.current_total_lift().get::() / 9.8 + > WingFlexTestBed::NOMINAL_WEIGHT_KG * 0.5 + ); + } + + #[test] + fn in_straight_flight_has_plane_lift_equal_to_weight() { + let mut test_bed = WingFlexTestBed::new().with_nominal_weight().in_1g_flight(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + assert!( + test_bed.current_total_lift().get::() / 9.8 + < WingFlexTestBed::NOMINAL_WEIGHT_KG * 1.1 + ); + assert!( + test_bed.current_total_lift().get::() / 9.8 + > WingFlexTestBed::NOMINAL_WEIGHT_KG * 0.9 + ); + } + + #[test] + fn in_straight_flight_at_max_fuel_has_plane_lift_equal_to_weight() { + let mut test_bed = WingFlexTestBed::new().with_max_fuel().in_1g_flight(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + assert!( + test_bed.current_total_lift().get::() / 9.8 + < WingFlexTestBed::NOMINAL_WEIGHT_KG * 1.1 + ); + assert!( + test_bed.current_total_lift().get::() / 9.8 + > WingFlexTestBed::NOMINAL_WEIGHT_KG * 0.9 + ); + } + + #[test] + fn in_left_turn_flight_has_more_right_lift() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .in_1g_flight() + .left_turn_ailerons() + .spoilers_left_turn(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + assert!( + test_bed.current_left_wing_lift().get::() + < test_bed.current_right_wing_lift().get::() + ); + } + + #[test] + fn in_right_turn_flight_has_more_left_lift() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .in_1g_flight() + .right_turn_ailerons() + .spoilers_right_turn(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + assert!( + test_bed.current_left_wing_lift().get::() + > test_bed.current_right_wing_lift().get::() + ); + } + + #[test] + fn in_straight_flight_all_left_lifts_all_right_lifts_equals_total_lift() { + let mut test_bed = WingFlexTestBed::new().with_nominal_weight().in_1g_flight(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + // One percent precision check on total lift value + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + <= test_bed.current_total_lift().get::() * 1.01 + ); + + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + >= test_bed.current_total_lift().get::() * 0.99 + ); + } + + #[test] + fn in_right_turn_all_left_lifts_all_right_lifts_equals_total_lift() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .in_1g_flight() + .right_turn_ailerons() + .spoilers_right_turn(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + // One percent precision check on total lift value + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + <= test_bed.current_total_lift().get::() * 1.01 + ); + + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + >= test_bed.current_total_lift().get::() * 0.99 + ); + } + + #[test] + fn in_left_turn_all_left_lifts_all_right_lifts_equals_total_lift() { + let mut test_bed = WingFlexTestBed::new() + .with_nominal_weight() + .in_1g_flight() + .left_turn_ailerons() + .spoilers_left_turn(); + + test_bed = test_bed.run_waiting_for(Duration::from_secs(1)); + + // One percent precision check on total lift value + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + <= test_bed.current_total_lift().get::() * 1.01 + ); + + assert!( + test_bed.left_wing_lift_per_node().sum() + test_bed.right_wing_lift_per_node().sum() + >= test_bed.current_total_lift().get::() * 0.99 + ); + } +} diff --git a/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/lib.rs b/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/lib.rs index 1b80f6605b8..2cd14e83bc9 100644 --- a/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/lib.rs +++ b/fbw-a380x/src/wasm/systems/a380_systems_wasm/src/lib.rs @@ -262,6 +262,11 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_aircraft_variable("AMBIENT WIND Y", "meter per second", 0)? .provides_aircraft_variable("AMBIENT WIND Z", "meter per second", 0)? .provides_aircraft_variable("ANTISKID BRAKES ACTIVE", "Bool", 0)? + .provides_aircraft_variable("CONTACT POINT COMPRESSION", "Percent", 0)? + .provides_aircraft_variable("CONTACT POINT COMPRESSION", "Percent", 1)? + .provides_aircraft_variable("CONTACT POINT COMPRESSION", "Percent", 2)? + .provides_aircraft_variable("CONTACT POINT COMPRESSION", "Percent", 3)? + .provides_aircraft_variable("CONTACT POINT COMPRESSION", "Percent", 4)? .provides_aircraft_variable("EXTERNAL POWER AVAILABLE", "Bool", 1)? .provides_aircraft_variable("FUEL TOTAL QUANTITY WEIGHT", "Pounds", 0)? .provides_aircraft_variable("FUELSYSTEM TANK QUANTITY", "gallons", 1)? @@ -278,6 +283,8 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_aircraft_variable("GEAR ANIMATION POSITION", "Percent", 0)? .provides_aircraft_variable("GEAR ANIMATION POSITION", "Percent", 1)? .provides_aircraft_variable("GEAR ANIMATION POSITION", "Percent", 2)? + .provides_aircraft_variable("GEAR ANIMATION POSITION", "Percent", 3)? + .provides_aircraft_variable("GEAR ANIMATION POSITION", "Percent", 4)? .provides_aircraft_variable("GEAR CENTER POSITION", "Percent", 0)? .provides_aircraft_variable("GEAR LEFT POSITION", "Percent", 0)? .provides_aircraft_variable("GEAR RIGHT POSITION", "Percent", 0)? @@ -286,6 +293,7 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_aircraft_variable("GPS GROUND SPEED", "Knots", 0)? .provides_aircraft_variable("GPS GROUND MAGNETIC TRACK", "Degrees", 0)? .provides_aircraft_variable("GPS GROUND TRUE TRACK", "Degrees", 0)? + .provides_aircraft_variable("INCIDENCE ALPHA", "Degrees", 0)? .provides_aircraft_variable("INDICATED ALTITUDE", "Feet", 0)? .provides_aircraft_variable("INTERACTIVE POINT OPEN:0", "Percent", 0)? .provides_aircraft_variable("INTERACTIVE POINT OPEN", "Percent", 2)? @@ -306,7 +314,9 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_aircraft_variable("PUSHBACK ANGLE", "Radians", 0)? .provides_aircraft_variable("SEA LEVEL PRESSURE", "Millibars", 0)? .provides_aircraft_variable("SIM ON GROUND", "Bool", 0)? + .provides_aircraft_variable("SURFACE TYPE", "Enum", 0)? .provides_aircraft_variable("TOTAL AIR TEMPERATURE", "celsius", 0)? + .provides_aircraft_variable("TOTAL WEIGHT", "Pounds", 0)? .provides_aircraft_variable("TRAILING EDGE FLAPS LEFT PERCENT", "Percent", 0)? .provides_aircraft_variable("TRAILING EDGE FLAPS RIGHT PERCENT", "Percent", 0)? .provides_aircraft_variable("TURB ENG CORRECTED N1", "Percent", 1)? @@ -323,10 +333,24 @@ async fn systems(mut gauge: msfs::Gauge) -> Result<(), Box> { .provides_aircraft_variable("VELOCITY BODY Y", "feet per second", 0)? .provides_aircraft_variable("VELOCITY BODY Z", "feet per second", 0)? .provides_aircraft_variable("VELOCITY WORLD Y", "feet per minute", 0)? - .provides_aircraft_variable("INCIDENCE ALPHA", "degree", 0)? .provides_aircraft_variable("ROTATION VELOCITY BODY X", "degree per second", 0)? .provides_aircraft_variable("ROTATION VELOCITY BODY Y", "degree per second", 0)? .provides_aircraft_variable("ROTATION VELOCITY BODY Z", "degree per second", 0)? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY X", + "radian per second squared", + 0, + )? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY Y", + "radian per second squared", + 0, + )? + .provides_aircraft_variable( + "ROTATION ACCELERATION BODY Z", + "radian per second squared", + 0, + )? .provides_aircraft_variable("PAYLOAD STATION WEIGHT", "Pounds", 1)? .provides_aircraft_variable("PAYLOAD STATION WEIGHT", "Pounds", 2)? .provides_aircraft_variable("PAYLOAD STATION WEIGHT", "Pounds", 3)? diff --git a/fbw-common/src/wasm/systems/systems/src/engine/mod.rs b/fbw-common/src/wasm/systems/systems/src/engine/mod.rs index 88bd5643989..9e8c37397b6 100644 --- a/fbw-common/src/wasm/systems/systems/src/engine/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/engine/mod.rs @@ -7,7 +7,6 @@ use crate::{ simulation::{SimulationElement, SimulationElementVisitor}, }; -pub mod engine_wing_flex; pub mod leap_engine; pub mod reverser_thrust; pub mod trent_engine; diff --git a/fbw-common/src/wasm/systems/systems/src/fuel/mod.rs b/fbw-common/src/wasm/systems/systems/src/fuel/mod.rs index 7f7f82cb1bd..5e270f14d2b 100644 --- a/fbw-common/src/wasm/systems/systems/src/fuel/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/fuel/mod.rs @@ -11,6 +11,9 @@ pub const FUEL_GALLONS_TO_KG: f64 = 3.039075693483925; pub trait FuelPayload { fn total_load(&self) -> Mass; fn fore_aft_center_of_gravity(&self) -> f64; + fn tank_mass(&self, _t: usize) -> Mass { + Mass::default() + } } pub trait FuelCG { @@ -96,6 +99,10 @@ impl FuelSystem { Vector3::zeros() } } + + pub fn tank_mass(&self, t: usize) -> Mass { + self.fuel_tanks[t].quantity() + } } impl SimulationElement for FuelSystem { fn accept(&mut self, visitor: &mut T) { diff --git a/fbw-common/src/wasm/systems/systems/src/hydraulic/flap_slat.rs b/fbw-common/src/wasm/systems/systems/src/hydraulic/flap_slat.rs index 5dbf50334f3..9e8b3692f22 100644 --- a/fbw-common/src/wasm/systems/systems/src/hydraulic/flap_slat.rs +++ b/fbw-common/src/wasm/systems/systems/src/hydraulic/flap_slat.rs @@ -11,7 +11,7 @@ use uom::si::{ angular_velocity::{radian_per_second, revolution_per_minute}, f64::*, pressure::psi, - ratio::ratio, + ratio::{percent, ratio}, torque::pound_force_inch, volume::{cubic_inch, gallon}, volume_rate::{gallon_per_minute, gallon_per_second}, @@ -136,6 +136,9 @@ pub struct FlapSlatAssembly { final_surface_angle_carac: [f64; 12], circuit_target_pressure: Pressure, + + left_position: Ratio, + right_position: Ratio, } impl FlapSlatAssembly { const LOW_PASS_FILTER_SURFACE_POSITION_TRANSIENT_TIME_CONSTANT: Duration = @@ -186,6 +189,8 @@ impl FlapSlatAssembly { synchro_gear_breakpoints, final_surface_angle_carac, circuit_target_pressure, + left_position: Ratio::default(), + right_position: Ratio::default(), } } @@ -223,6 +228,8 @@ impl FlapSlatAssembly { ); self.update_motors_flow(context); + + self.update_position_ratios(); } fn update_speed_and_position(&mut self, context: &UpdateContext) { @@ -390,6 +397,23 @@ impl FlapSlatAssembly { self.left_motor.update_flow(context); } + fn update_position_ratios(&mut self) { + self.left_position = Ratio::new::( + interpolation( + &self.synchro_gear_breakpoints, + &self.final_surface_angle_carac, + self.position_feedback().get::(), + ) / interpolation( + &self.synchro_gear_breakpoints, + &self.final_surface_angle_carac, + self.max_synchro_gear_position.get::(), + ), + ); + + // TODO update when left and right are simulated separatly + self.right_position = self.left_position; + } + fn is_approaching_requested_position(&self, synchro_gear_angle_request: Angle) -> bool { self.speed.get::() > 0. && synchro_gear_angle_request - self.position_feedback() @@ -454,32 +478,24 @@ impl FlapSlatAssembly { fn is_surface_moving(&self) -> bool { self.speed.abs().get::() > Self::MIN_ANGULAR_SPEED_TO_REPORT_MOVING } + + pub fn left_position(&self) -> f64 { + self.left_position.get::() + } + + pub fn right_position(&self) -> f64 { + self.right_position.get::() + } } impl SimulationElement for FlapSlatAssembly { fn write(&self, writer: &mut SimulatorWriter) { writer.write( &self.position_left_percent_id, - interpolation( - &self.synchro_gear_breakpoints, - &self.final_surface_angle_carac, - self.position_feedback().get::(), - ) / interpolation( - &self.synchro_gear_breakpoints, - &self.final_surface_angle_carac, - self.max_synchro_gear_position.get::(), - ) * 100., + self.left_position.get::(), ); writer.write( &self.position_right_percent_id, - interpolation( - &self.synchro_gear_breakpoints, - &self.final_surface_angle_carac, - self.position_feedback().get::(), - ) / interpolation( - &self.synchro_gear_breakpoints, - &self.final_surface_angle_carac, - self.max_synchro_gear_position.get::(), - ) * 100., + self.right_position.get::(), ); let flaps_surface_angle = self.flap_surface_angle(); diff --git a/fbw-common/src/wasm/systems/systems/src/hydraulic/mod.rs b/fbw-common/src/wasm/systems/systems/src/hydraulic/mod.rs index e26a23c5eb3..b7bc302c94a 100644 --- a/fbw-common/src/wasm/systems/systems/src/hydraulic/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/hydraulic/mod.rs @@ -2144,7 +2144,8 @@ impl FluidPhysics { } fn update(&mut self, context: &UpdateContext) { - self.wobble_physics.update(context); + self.wobble_physics + .update(context, Vector3::default(), Vector3::default()); self.g_trap_is_empty .update(context, self.is_fluid_going_up()); diff --git a/fbw-common/src/wasm/systems/systems/src/landing_gear/mod.rs b/fbw-common/src/wasm/systems/systems/src/landing_gear/mod.rs index 23b90f5ddfd..73017e9f8c9 100644 --- a/fbw-common/src/wasm/systems/systems/src/landing_gear/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/landing_gear/mod.rs @@ -154,9 +154,9 @@ impl LandingGear { left_compression_id: context.get_identifier(Self::GEAR_LEFT_COMPRESSION.to_owned()), right_compression_id: context.get_identifier(Self::GEAR_RIGHT_COMPRESSION.to_owned()), - center_compression: Ratio::new::(0.), - left_compression: Ratio::new::(0.), - right_compression: Ratio::new::(0.), + center_compression: Ratio::default(), + left_compression: Ratio::default(), + right_compression: Ratio::default(), } } diff --git a/fbw-common/src/wasm/systems/systems/src/navigation/adirs.rs b/fbw-common/src/wasm/systems/systems/src/navigation/adirs.rs index d5e488990f5..5760becd04f 100644 --- a/fbw-common/src/wasm/systems/systems/src/navigation/adirs.rs +++ b/fbw-common/src/wasm/systems/systems/src/navigation/adirs.rs @@ -1862,6 +1862,7 @@ mod tests { use std::time::Duration; use uom::si::{ angle::degree, + angular_velocity::radian_per_second, length::foot, ratio::percent, thermodynamic_temperature::degree_celsius, @@ -3551,8 +3552,8 @@ mod tests { .body_pitch_rate(adiru_number) .normal_value() .unwrap() - .get::(), - -rate.get::() + .get::(), + -rate.get::() ); } diff --git a/fbw-common/src/wasm/systems/systems/src/physics/mod.rs b/fbw-common/src/wasm/systems/systems/src/physics/mod.rs index eb49d3d4453..819b85cdc6c 100644 --- a/fbw-common/src/wasm/systems/systems/src/physics/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/physics/mod.rs @@ -1,4 +1,7 @@ -use crate::{shared::random_from_normal_distribution, simulation::UpdateContext}; +use crate::{ + shared::local_acceleration_at_plane_coordinate, shared::random_from_normal_distribution, + simulation::UpdateContext, +}; use uom::si::{f64::*, mass::kilogram}; @@ -53,6 +56,7 @@ impl SpringPhysics { pub enum GravityEffect { NoGravity, GravityFiltered, + ExternalAccelerationOnly, } pub struct WobblePhysics { @@ -101,20 +105,36 @@ impl WobblePhysics { } } - pub fn update(&mut self, context: &UpdateContext) { - let acceleration = self.update_forces(context) / self.virtual_mass.get::(); + pub fn update( + &mut self, + context: &UpdateContext, + external_acceleration: Vector3, + offset_point_coordinates: Vector3, + ) { + let acceleration = + self.update_forces(context, external_acceleration, offset_point_coordinates) + / self.virtual_mass.get::(); self.cg_speed += acceleration * context.delta_as_secs_f64(); self.cg_position += self.cg_speed * context.delta_as_secs_f64(); } - fn update_forces(&mut self, context: &UpdateContext) -> Vector3 { + fn update_forces( + &mut self, + context: &UpdateContext, + external_acceleration: Vector3, + offset_point_coordinates: Vector3, + ) -> Vector3 { let local_acceleration = match self.gravity_effect { - GravityEffect::NoGravity => context.local_acceleration_without_gravity(), + GravityEffect::NoGravity => { + context.local_acceleration_without_gravity() + external_acceleration + - local_acceleration_at_plane_coordinate(context, offset_point_coordinates) + } GravityEffect::GravityFiltered => { - context.acceleration_plane_reference_filtered_ms2_vector() + context.acceleration_plane_reference_filtered_ms2_vector() + external_acceleration } + GravityEffect::ExternalAccelerationOnly => external_acceleration, }; let acceleration_force = local_acceleration * self.virtual_mass.get::(); 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 7490b42438e..eb912aaee25 100644 --- a/fbw-common/src/wasm/systems/systems/src/shared/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/shared/mod.rs @@ -336,6 +336,16 @@ impl Display for AirbusElectricPumpId { } } +/// Access to all aircraft surfaces positions +pub trait SurfacesPositions { + fn left_spoilers_positions(&self) -> &[f64]; + fn right_spoilers_positions(&self) -> &[f64]; + fn left_ailerons_positions(&self) -> &[f64]; + fn right_ailerons_positions(&self) -> &[f64]; + fn left_flaps_position(&self) -> f64; + fn right_flaps_position(&self) -> f64; +} + /// The common types of electrical buses within Airbus aircraft. /// These include types such as AC, DC, AC ESS, etc. #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] @@ -705,6 +715,40 @@ pub fn height_over_ground( Length::new::(offset_including_plane_rotation[1]) + context.plane_height_over_ground() } +// Gets the local acceleration at a point away from plane reference point, including rotational effects (tangential/centripetal) +// Warning: It EXLCLUDES PLANE LOCAL CG ACCELERATION. Add to plane acceleration to have total local acceleration at this point +// +// For reference rotational velocity and acceleration from MSFS are: +// X axis pitch up negative +// Y axis yaw left negative +// Z axis roll right negative +// +// Acceleration returned is local to plane reference with +// X negative left positive right +// Y negative down positive up +// Z negative aft positive forward +pub fn local_acceleration_at_plane_coordinate( + context: &UpdateContext, + offset_from_plane_reference: Vector3, +) -> Vector3 { + // If less than 10cm from center of rotation we don't consider rotational effect + if offset_from_plane_reference.norm() < 0.01 { + return Vector3::default(); + } + + let tangential_velocity_of_point = + offset_from_plane_reference.cross(&-context.rotation_velocity_rad_s()); + let tangential_acceleration_of_point = + offset_from_plane_reference.cross(&-context.rotation_acceleration_rad_s2()); + + let radial_norm_vector = -offset_from_plane_reference.normalize(); + + let centripetal_acceleration = radial_norm_vector + * (tangential_velocity_of_point.norm().powi(2) / offset_from_plane_reference.norm()); + + centripetal_acceleration + tangential_acceleration_of_point +} + pub struct InternationalStandardAtmosphere; impl InternationalStandardAtmosphere { const TEMPERATURE_LAPSE_RATE: f64 = 0.0065; @@ -1661,3 +1705,184 @@ mod height_over_ground { test_bed.run_with_delta(Duration::from_secs(0)); } } + +#[cfg(test)] +mod local_acceleration_at_plane_coordinate { + use uom::si::angular_velocity::{degree_per_second, radian_per_second}; + + use super::*; + + use crate::simulation::{ + test::{ElementCtorFn, SimulationTestBed, WriteByName}, + SimulationElement, + }; + + #[derive(Default)] + struct RotatingObject { + local_accel: Vector3, + rotating_point_position: Vector3, + } + impl RotatingObject { + fn default() -> Self { + Self { + local_accel: Vector3::default(), + rotating_point_position: Vector3::default(), + } + } + + fn update(&mut self, context: &UpdateContext) { + self.local_accel = + local_acceleration_at_plane_coordinate(context, self.rotating_point_position); + } + + fn set_point_position(&mut self, rotating_point_position: Vector3) { + self.rotating_point_position = rotating_point_position; + } + } + impl SimulationElement for RotatingObject {} + + #[test] + fn pilot_cabin_acceleration_pitch_rotations() { + let mut test_bed = SimulationTestBed::from(ElementCtorFn(|_| RotatingObject::default())) + .with_update_after_power_distribution(|e, context| { + e.update(context); + }); + + // Assuming pilot cabin is 1m forward for simplicity + let cabin_position = Vector3::new(0., 0., 1.); + test_bed.command_element(|e| e.set_point_position(cabin_position)); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + + // Pitch up accel + test_bed.write_by_name("ROTATION VELOCITY BODY X", 0.); + test_bed.write_by_name("ROTATION ACCELERATION BODY X", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(0., 1., 0.))); + + // Pitch up accel with velocity adds centripetal force + test_bed.write_by_name( + "ROTATION VELOCITY BODY X", + AngularVelocity::new::(-1.).get::(), + ); + test_bed.write_by_name("ROTATION ACCELERATION BODY X", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(0., 1., -1.))); + } + + #[test] + fn pilot_cabin_acceleration_yaw_rotations() { + let mut test_bed = SimulationTestBed::from(ElementCtorFn(|_| RotatingObject::default())) + .with_update_after_power_distribution(|e, context| { + e.update(context); + }); + + // Assuming pilot cabin is 1m forward for simplicity + let cabin_position = Vector3::new(0., 0., 1.); + test_bed.command_element(|e| e.set_point_position(cabin_position)); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + + // Yaw right accel + test_bed.write_by_name("ROTATION VELOCITY BODY Y", 0.); + test_bed.write_by_name("ROTATION ACCELERATION BODY Y", 1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(1., 0., 0.))); + + // Yaw right accel with velocity adds centripetal force + test_bed.write_by_name( + "ROTATION VELOCITY BODY Y", + AngularVelocity::new::(1.).get::(), + ); + test_bed.write_by_name("ROTATION ACCELERATION BODY Y", 1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(1., 0., -1.))); + + // Yaw left accel + test_bed.write_by_name("ROTATION VELOCITY BODY Y", 0.); + test_bed.write_by_name("ROTATION ACCELERATION BODY Y", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(-1., 0., 0.))); + + // Yaw left accel with velocity adds centripetal force + test_bed.write_by_name( + "ROTATION VELOCITY BODY Y", + AngularVelocity::new::(-1.).get::(), + ); + test_bed.write_by_name("ROTATION ACCELERATION BODY Y", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(-1., 0., -1.))); + } + + #[test] + fn pilot_cabin_acceleration_roll_rotations() { + let mut test_bed = SimulationTestBed::from(ElementCtorFn(|_| RotatingObject::default())) + .with_update_after_power_distribution(|e, context| { + e.update(context); + }); + + // Assuming pilot cabin is 1m forward for simplicity + let cabin_position = Vector3::new(0., 0., 1.); + test_bed.command_element(|e| e.set_point_position(cabin_position)); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + + // roll right accel -> Aligned on roll axis we expect no effect + test_bed.write_by_name("ROTATION VELOCITY BODY Z", 0.); + test_bed.write_by_name("ROTATION ACCELERATION BODY Z", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + + // roll right accel with velocity -> Aligned on roll axis we expect no effect + test_bed.write_by_name( + "ROTATION VELOCITY BODY Z", + AngularVelocity::new::(-1.).get::(), + ); + test_bed.write_by_name("ROTATION ACCELERATION BODY Z", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + } + + #[test] + fn right_wing_acceleration_roll_rotations() { + let mut test_bed = SimulationTestBed::from(ElementCtorFn(|_| RotatingObject::default())) + .with_update_after_power_distribution(|e, context| { + e.update(context); + }); + + // Assuming right wing is 1m right for simplicity + let right_wing_position = Vector3::new(1., 0., 0.); + test_bed.command_element(|e| e.set_point_position(right_wing_position)); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::default())); + + // roll right accel -> expect down accel + test_bed.write_by_name("ROTATION VELOCITY BODY Z", 0.); + test_bed.write_by_name("ROTATION ACCELERATION BODY Z", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(0., -1., 0.))); + + // roll right accel with velocity -> Down Force plus centripetal left + test_bed.write_by_name( + "ROTATION VELOCITY BODY Z", + AngularVelocity::new::(-1.).get::(), + ); + test_bed.write_by_name("ROTATION ACCELERATION BODY Z", -1.); + + test_bed.run_with_delta(Duration::from_secs(0)); + assert!(test_bed.query_element(|e| e.local_accel == Vector3::new(-1., -1., 0.))); + } +} 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 f2a1947a77d..70c0f097d30 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 @@ -2,6 +2,8 @@ use std::time::Duration; use uom::si::{ acceleration::meter_per_second_squared, angle::radian, + angular_acceleration::radian_per_second_squared, + angular_velocity::{degree_per_second, radian_per_second}, f64::*, length::millimeter, mass_density::kilogram_per_cubic_meter, @@ -50,6 +52,65 @@ impl Attitude { } } +#[derive(Clone, Copy, Debug)] +pub enum SurfaceTypeMsfs { + Concrete = 0, + Grass = 1, + Water = 2, + GrassBumpy = 3, + Asphalt = 4, + ShortGrass = 5, + LongGrass = 6, + HardTurf = 7, + Snow = 8, + Ice = 9, + Urban = 10, + Forest = 11, + Dirt = 12, + Coral = 13, + Gravel = 14, + OilTreated = 15, + SteelMats = 16, + Bituminus = 17, + Brick = 18, + Macadam = 19, + Planks = 20, + Sand = 21, + Shale = 22, + Tarmac = 23, +} +impl From for SurfaceTypeMsfs { + fn from(value: f64) -> Self { + match value.floor() as u32 { + 0 => SurfaceTypeMsfs::Concrete, + 1 => SurfaceTypeMsfs::Grass, + 2 => SurfaceTypeMsfs::Water, + 3 => SurfaceTypeMsfs::GrassBumpy, + 4 => SurfaceTypeMsfs::Asphalt, + 5 => SurfaceTypeMsfs::ShortGrass, + 6 => SurfaceTypeMsfs::LongGrass, + 7 => SurfaceTypeMsfs::HardTurf, + 8 => SurfaceTypeMsfs::Snow, + 9 => SurfaceTypeMsfs::Ice, + 10 => SurfaceTypeMsfs::Urban, + 11 => SurfaceTypeMsfs::Forest, + 12 => SurfaceTypeMsfs::Dirt, + 13 => SurfaceTypeMsfs::Coral, + 14 => SurfaceTypeMsfs::Gravel, + 15 => SurfaceTypeMsfs::OilTreated, + 16 => SurfaceTypeMsfs::SteelMats, + 17 => SurfaceTypeMsfs::Bituminus, + 18 => SurfaceTypeMsfs::Brick, + 19 => SurfaceTypeMsfs::Macadam, + 20 => SurfaceTypeMsfs::Planks, + 21 => SurfaceTypeMsfs::Sand, + 22 => SurfaceTypeMsfs::Shale, + 23 => SurfaceTypeMsfs::Tarmac, + i => panic!("Cannot convert from {} to SurfaceTypeMsfs.", i), + } + } +} + #[derive(Clone, Copy, Debug, Default)] pub struct LocalAcceleration { acceleration: [Acceleration; 3], @@ -167,6 +228,13 @@ pub struct UpdateContext { total_yaw_inertia_id: VariableIdentifier, precipitation_rate_id: VariableIdentifier, in_cloud_id: VariableIdentifier, + surface_id: VariableIdentifier, + rotation_acc_x_id: VariableIdentifier, + rotation_acc_y_id: VariableIdentifier, + rotation_acc_z_id: VariableIdentifier, + rotation_vel_x_id: VariableIdentifier, + rotation_vel_y_id: VariableIdentifier, + rotation_vel_z_id: VariableIdentifier, delta: Delta, simulation_time: f64, @@ -181,6 +249,7 @@ pub struct UpdateContext { vertical_speed: Velocity, local_acceleration: LocalAcceleration, + local_acceleration_plane_reference: Vector3, local_acceleration_plane_reference_filtered: LowPassFilter>, world_ambient_wind: Velocity3D, @@ -200,6 +269,11 @@ pub struct UpdateContext { precipitation_rate: Length, in_cloud: bool, + + surface: SurfaceTypeMsfs, + + rotation_accel: Vector3, + rotation_vel: Vector3, } impl UpdateContext { pub(crate) const IS_READY_KEY: &'static str = "IS_READY"; @@ -231,6 +305,13 @@ impl UpdateContext { pub(crate) const LATITUDE_KEY: &'static str = "PLANE LATITUDE"; pub(crate) const TOTAL_WEIGHT_KEY: &'static str = "TOTAL WEIGHT"; pub(crate) const TOTAL_YAW_INERTIA: &'static str = "TOTAL WEIGHT YAW MOI"; + pub(crate) const SURFACE_KEY: &'static str = "SURFACE TYPE"; + pub(crate) const ROTATION_ACCEL_X_KEY: &'static str = "ROTATION ACCELERATION BODY X"; + pub(crate) const ROTATION_ACCEL_Y_KEY: &'static str = "ROTATION ACCELERATION BODY Y"; + pub(crate) const ROTATION_ACCEL_Z_KEY: &'static str = "ROTATION ACCELERATION BODY Z"; + pub(crate) const ROTATION_VEL_X_KEY: &'static str = "ROTATION VELOCITY BODY X"; + pub(crate) const ROTATION_VEL_Y_KEY: &'static str = "ROTATION VELOCITY BODY Y"; + pub(crate) const ROTATION_VEL_Z_KEY: &'static str = "ROTATION VELOCITY BODY Z"; // Plane accelerations can become crazy with msfs collision handling. // Having such filtering limits high frequencies transients in accelerations used for physics @@ -295,6 +376,14 @@ impl UpdateContext { precipitation_rate_id: context.get_identifier(Self::AMBIENT_PRECIP_RATE_KEY.to_owned()), in_cloud_id: context.get_identifier(Self::IN_CLOUD_KEY.to_owned()), + surface_id: context.get_identifier(Self::SURFACE_KEY.to_owned()), + rotation_acc_x_id: context.get_identifier(Self::ROTATION_ACCEL_X_KEY.to_owned()), + rotation_acc_y_id: context.get_identifier(Self::ROTATION_ACCEL_Y_KEY.to_owned()), + rotation_acc_z_id: context.get_identifier(Self::ROTATION_ACCEL_Z_KEY.to_owned()), + rotation_vel_x_id: context.get_identifier(Self::ROTATION_VEL_X_KEY.to_owned()), + rotation_vel_y_id: context.get_identifier(Self::ROTATION_VEL_Y_KEY.to_owned()), + rotation_vel_z_id: context.get_identifier(Self::ROTATION_VEL_Z_KEY.to_owned()), + delta: delta.into(), simulation_time, is_ready: true, @@ -311,6 +400,7 @@ impl UpdateContext { vertical_acceleration, longitudinal_acceleration, ), + local_acceleration_plane_reference: Vector3::new(0., -9.8, 0.), local_acceleration_plane_reference_filtered: LowPassFilter::>::new_with_init_value( Self::PLANE_ACCELERATION_FILTERING_TIME_CONSTANT, @@ -342,6 +432,11 @@ impl UpdateContext { total_yaw_inertia_slug_foot_squared: 10., precipitation_rate: Length::default(), in_cloud: false, + + surface: SurfaceTypeMsfs::Asphalt, + + rotation_accel: Vector3::default(), + rotation_vel: Vector3::default(), } } @@ -377,6 +472,15 @@ impl UpdateContext { precipitation_rate_id: context.get_identifier("AMBIENT PRECIP RATE".to_owned()), in_cloud_id: context.get_identifier("AMBIENT IN CLOUD".to_owned()), + surface_id: context.get_identifier("SURFACE TYPE".to_owned()), + + rotation_acc_x_id: context.get_identifier(Self::ROTATION_ACCEL_X_KEY.to_owned()), + rotation_acc_y_id: context.get_identifier(Self::ROTATION_ACCEL_Y_KEY.to_owned()), + rotation_acc_z_id: context.get_identifier(Self::ROTATION_ACCEL_Z_KEY.to_owned()), + rotation_vel_x_id: context.get_identifier(Self::ROTATION_VEL_X_KEY.to_owned()), + rotation_vel_y_id: context.get_identifier(Self::ROTATION_VEL_Y_KEY.to_owned()), + rotation_vel_z_id: context.get_identifier(Self::ROTATION_VEL_Z_KEY.to_owned()), + delta: Default::default(), simulation_time: Default::default(), is_ready: Default::default(), @@ -389,7 +493,7 @@ impl UpdateContext { is_on_ground: Default::default(), vertical_speed: Default::default(), local_acceleration: Default::default(), - + local_acceleration_plane_reference: Vector3::new(0., -9.8, 0.), local_acceleration_plane_reference_filtered: LowPassFilter::>::new_with_init_value( Self::PLANE_ACCELERATION_FILTERING_TIME_CONSTANT, @@ -421,6 +525,11 @@ impl UpdateContext { total_yaw_inertia_slug_foot_squared: 1., precipitation_rate: Length::default(), in_cloud: false, + + surface: SurfaceTypeMsfs::Asphalt, + + rotation_accel: Vector3::default(), + rotation_vel: Vector3::default(), } } @@ -488,6 +597,27 @@ impl UpdateContext { self.in_cloud = reader.read(&self.in_cloud_id); + let surface_read: f64 = reader.read(&self.surface_id); + self.surface = surface_read.into(); + + self.rotation_accel = Vector3::new( + AngularAcceleration::new::( + reader.read(&self.rotation_acc_x_id), + ), + AngularAcceleration::new::( + reader.read(&self.rotation_acc_y_id), + ), + AngularAcceleration::new::( + reader.read(&self.rotation_acc_z_id), + ), + ); + + self.rotation_vel = Vector3::new( + AngularVelocity::new::(reader.read(&self.rotation_vel_x_id)), + AngularVelocity::new::(reader.read(&self.rotation_vel_y_id)), + AngularVelocity::new::(reader.read(&self.rotation_vel_z_id)), + ); + self.update_relative_wind(); self.update_local_acceleration_plane_reference(delta); @@ -506,12 +636,12 @@ impl UpdateContext { // Total acceleration in plane reference is the gravity in world reference rotated to plane reference. To this we substract // the local plane reference to get final local acceleration (if plane falling at 1G final local accel is 1G of gravity - 1G local accel = 0G) - let total_acceleration_plane_reference = (pitch_rotation + self.local_acceleration_plane_reference = (pitch_rotation * (bank_rotation * gravity_acceleration_world_reference)) - plane_acceleration_plane_reference; self.local_acceleration_plane_reference_filtered - .update(delta, total_acceleration_plane_reference); + .update(delta, self.local_acceleration_plane_reference); } /// Relative wind could be directly read from simvar RELATIVE WIND VELOCITY XYZ. @@ -631,8 +761,14 @@ impl UpdateContext { self.local_acceleration.vert_accel() } + pub fn surface_type(&self) -> SurfaceTypeMsfs { + self.surface + } + pub fn local_acceleration_without_gravity(&self) -> Vector3 { - self.local_acceleration.to_ms2_vector() + // Gives the local acceleration in plane reference. If msfs local accel is free falling -9.81 + // then it's locally a up acceleration. + -self.local_acceleration.to_ms2_vector() } pub fn local_relative_wind(&self) -> Velocity3D { @@ -643,14 +779,14 @@ impl UpdateContext { self.local_velocity } - pub fn acceleration(&self) -> LocalAcceleration { - self.local_acceleration - } - pub fn acceleration_plane_reference_filtered_ms2_vector(&self) -> Vector3 { self.local_acceleration_plane_reference_filtered.output() } + pub fn acceleration_plane_reference_unfiltered_ms2_vector(&self) -> Vector3 { + self.local_acceleration_plane_reference + } + pub fn pitch(&self) -> Angle { self.attitude.pitch() } @@ -690,6 +826,22 @@ impl UpdateContext { self.total_yaw_inertia_slug_foot_squared * Self::SLUG_FOOT_SQUARED_TO_KG_METER_SQUARED_CONVERSION } + + pub fn rotation_acceleration_rad_s2(&self) -> Vector3 { + Vector3::new( + self.rotation_accel[0].get::(), + self.rotation_accel[1].get::(), + self.rotation_accel[2].get::(), + ) + } + + pub fn rotation_velocity_rad_s(&self) -> Vector3 { + Vector3::new( + self.rotation_vel[0].get::(), + self.rotation_vel[1].get::(), + self.rotation_vel[2].get::(), + ) + } } impl DeltaContext for UpdateContext { diff --git a/fbw-common/src/wasm/systems/systems/src/structural_flex/elevator_flex.rs b/fbw-common/src/wasm/systems/systems/src/structural_flex/elevator_flex.rs index bd7aaeeae70..f88d4b9e5e9 100644 --- a/fbw-common/src/wasm/systems/systems/src/structural_flex/elevator_flex.rs +++ b/fbw-common/src/wasm/systems/systems/src/structural_flex/elevator_flex.rs @@ -9,7 +9,7 @@ use crate::{ }, }; -use uom::si::{f64::*, torque::newton_meter}; +use uom::si::{acceleration::meter_per_second_squared, f64::*, torque::newton_meter}; use nalgebra::Vector3; use std::fmt::Debug; @@ -36,6 +36,8 @@ impl AftConeFlexPhysics { // stronger it is to flex the rudder relative to lower surface const AERODYNAMIC_FLEX_COEF_FOR_UPPER_RUDDER_SURFACE: f64 = 1.5; + const SURFACE_VIBRATION_SENSITIVITY: f64 = 1.0; + fn new(context: &mut InitContext) -> Self { Self { position_id: context.get_identifier("AFT_FLEX_POSITION".to_owned()), @@ -47,20 +49,34 @@ impl AftConeFlexPhysics { 5., 40000., 100., - 150., + 300., 10., - Vector3::new(200., 50., 200.), + Vector3::new(200., 200., 200.), 5., ), - position_output_gain: 5., + position_output_gain: 2., animation_position: 0.5, } } - fn update(&mut self, context: &UpdateContext, rudder_aero_torques: (Torque, Torque)) { - self.wobble_physics.update(context); + fn update( + &mut self, + context: &UpdateContext, + rudder_aero_torques: (Torque, Torque), + surface_vibration_acceleration: Acceleration, + ) { + self.wobble_physics.update( + context, + Vector3::new( + 0., + (surface_vibration_acceleration * Self::SURFACE_VIBRATION_SENSITIVITY) + .get::(), + 0., + ), + Vector3::new(0., 3.4, -25.), + ); self.update_animation_position(rudder_aero_torques); } @@ -114,6 +130,8 @@ impl ElevatorFlexPhysics { // stronger it is to flex the elevator relative to inner surface const AERODYNAMIC_FLEX_COEF_FOR_OUTER_ELEVATOR_SURFACE: f64 = 1.5; + const SURFACE_VIBRATION_SENSITIVITY: f64 = 1.0; + fn new(context: &mut InitContext, side: ElevatorSide) -> Self { Self { y_position_id: if side == ElevatorSide::Left { @@ -136,13 +154,13 @@ impl ElevatorFlexPhysics { 5., 40000., 100., - 150., + 250., 10., - Vector3::new(200., 50., 200.), + Vector3::new(200., 100., 200.), 5., ), - position_output_gain: 3., + position_output_gain: 2.5, animation_position: 0.5, @@ -153,11 +171,21 @@ impl ElevatorFlexPhysics { fn update( &mut self, context: &UpdateContext, - outter_inner_elevator_aero_torques: (Torque, Torque), + outer_inner_elevator_aero_torques: (Torque, Torque), + surface_vibration_acceleration: Acceleration, ) { - self.wobble_physics.update(context); + self.wobble_physics.update( + context, + Vector3::new( + 0., + (surface_vibration_acceleration * Self::SURFACE_VIBRATION_SENSITIVITY) + .get::(), + 0., + ), + Vector3::new(0., 3.4, -28.2), + ); - self.update_animation_position(outter_inner_elevator_aero_torques); + self.update_animation_position(outer_inner_elevator_aero_torques); } fn update_animation_position(&mut self, elevator_aero_torques: (Torque, Torque)) { @@ -239,8 +267,9 @@ impl FlexibleElevators { pub fn update( &mut self, context: &UpdateContext, - outter_inner_elevator_aero_torques: [(Torque, Torque); 2], + outer_inner_elevator_aero_torques: [(Torque, Torque); 2], up_down_rudder_aero_torques: (Torque, Torque), + surface_vibration_acceleration: Acceleration, ) { self.flex_updater.update(context); @@ -248,13 +277,15 @@ impl FlexibleElevators { for (idx, elevator_flex) in &mut self.elevators_flex.iter_mut().enumerate() { elevator_flex.update( &context.with_delta(cur_time_step), - outter_inner_elevator_aero_torques[idx], + outer_inner_elevator_aero_torques[idx], + surface_vibration_acceleration, ); } self.aft_cone_flex.update( &context.with_delta(cur_time_step), up_down_rudder_aero_torques, + surface_vibration_acceleration, ); } } @@ -283,7 +314,7 @@ mod tests { struct EngineFlexTestAircraft { elevators_flex: FlexibleElevators, - outter_elevator_aero_torque: Torque, + outer_elevator_aero_torque: Torque, inner_elevator_aero_torque: Torque, down_rudder_aero_torque: Torque, @@ -294,7 +325,7 @@ mod tests { Self { elevators_flex: FlexibleElevators::new(context), - outter_elevator_aero_torque: Torque::default(), + outer_elevator_aero_torque: Torque::default(), inner_elevator_aero_torque: Torque::default(), down_rudder_aero_torque: Torque::default(), up_rudder_aero_torque: Torque::default(), @@ -306,15 +337,16 @@ mod tests { context, [ ( - self.outter_elevator_aero_torque, + self.outer_elevator_aero_torque, self.inner_elevator_aero_torque, ), ( - self.outter_elevator_aero_torque, + self.outer_elevator_aero_torque, self.inner_elevator_aero_torque, ), ], (self.up_rudder_aero_torque, self.down_rudder_aero_torque), + Acceleration::default(), ); } } @@ -345,9 +377,11 @@ mod tests { let left_position: f64 = test_bed.read_by_name("ELEVATOR_LEFT_WOBBLE_Y_POSITION"); let right_position: f64 = test_bed.read_by_name("ELEVATOR_RIGHT_WOBBLE_Y_POSITION"); + let aft_position: f64 = test_bed.read_by_name("AFT_FLEX_POSITION"); assert_about_eq!(left_position, 0.5); assert_about_eq!(right_position, 0.5); + assert_about_eq!(aft_position, 0.5); } // Following test is ignored because it's hard to set static boundaries to desired results diff --git a/fbw-common/src/wasm/systems/systems/src/engine/engine_wing_flex.rs b/fbw-common/src/wasm/systems/systems/src/structural_flex/engine_wobble.rs similarity index 90% rename from fbw-common/src/wasm/systems/systems/src/engine/engine_wing_flex.rs rename to fbw-common/src/wasm/systems/systems/src/structural_flex/engine_wobble.rs index c02e0657487..72e746045a8 100644 --- a/fbw-common/src/wasm/systems/systems/src/engine/engine_wing_flex.rs +++ b/fbw-common/src/wasm/systems/systems/src/structural_flex/engine_wobble.rs @@ -11,6 +11,7 @@ use crate::{ use nalgebra::Vector3; use std::fmt::Debug; +use uom::si::{acceleration::meter_per_second_squared, f64::*}; /// Solves a basic mass connected to a static point through a spring damper system /// Mass center of gravity position reacting to external accelerations is then used to model engine wobbling movement @@ -45,26 +46,34 @@ impl EngineFlexPhysics { dev_mode_enable_id: context.get_identifier("ENGINE_WOBBLE_DEV_ENABLE".to_owned()), wobble_physics: WobblePhysics::new( - GravityEffect::NoGravity, + GravityEffect::ExternalAccelerationOnly, Vector3::default(), 2000., 100., 800000., 50000., - 500., - 20., - Vector3::new(500., 500., 500.), + 1500., + 100., + Vector3::new(1000., 1000., 1000.), 50., ), - position_output_gain: 90., + position_output_gain: 70., animation_position: 0.5, } } - pub fn update(&mut self, context: &UpdateContext) { - self.wobble_physics.update(context); + pub fn update(&mut self, context: &UpdateContext, wing_pylon_acceleration: Acceleration) { + self.wobble_physics.update( + context, + Vector3::new( + 0., + wing_pylon_acceleration.get::(), + 0., + ), + Vector3::default(), + ); self.update_animation_position(); } @@ -132,12 +141,14 @@ impl EnginesFlexiblePhysics { } } - pub fn update(&mut self, context: &UpdateContext) { + pub fn update(&mut self, context: &UpdateContext, pylons_accelerations: [Acceleration; N]) { self.engines_flex_updater.update(context); for cur_time_step in self.engines_flex_updater { - for engine_flex in &mut self.engines_flex { - engine_flex.update(&context.with_delta(cur_time_step)); + for (engine_flex, pylons_acceleration) in + &mut self.engines_flex.iter_mut().zip(pylons_accelerations) + { + engine_flex.update(&context.with_delta(cur_time_step), pylons_acceleration); } } } @@ -173,7 +184,8 @@ mod tests { } fn update(&mut self, context: &UpdateContext) { - self.engines_flex.update(context); + self.engines_flex + .update(context, [Acceleration::default(); 4]); } } impl Aircraft for EngineFlexTestAircraft { diff --git a/fbw-common/src/wasm/systems/systems/src/structural_flex/mod.rs b/fbw-common/src/wasm/systems/systems/src/structural_flex/mod.rs index 663677dc39e..35fffcb3787 100644 --- a/fbw-common/src/wasm/systems/systems/src/structural_flex/mod.rs +++ b/fbw-common/src/wasm/systems/systems/src/structural_flex/mod.rs @@ -1 +1,119 @@ pub mod elevator_flex; +pub mod engine_wobble; +pub mod wing_flex; + +use crate::shared::low_pass_filter::LowPassFilter; +use crate::simulation::{SurfaceTypeMsfs, UpdateContext}; + +use crate::shared::{random_from_normal_distribution, random_from_range}; + +use uom::si::{ + acceleration::meter_per_second_squared, + f64::*, + velocity::{knot, meter_per_second}, +}; + +use std::time::Duration; + +struct BumpGenerator { + bump_accel: Acceleration, + + seconds_per_bump: Duration, + + mean_bump: f64, + std_bump: f64, +} +impl BumpGenerator { + fn new(seconds_per_bump: Duration, mean_bump: f64, std_bump: f64) -> Self { + Self { + bump_accel: Acceleration::default(), + seconds_per_bump, + + mean_bump, + std_bump, + } + } + + fn update(&mut self, context: &UpdateContext) { + let bump_encountered = random_from_range(0., 1.) + < context.delta_as_secs_f64() / self.seconds_per_bump.as_secs_f64(); + + if bump_encountered { + self.bump_accel = Acceleration::new::( + random_from_normal_distribution(self.mean_bump, self.std_bump), + ); + + if random_from_range(0., 1.) < 0.5 { + self.bump_accel = -self.bump_accel; + } + } else { + self.bump_accel = Acceleration::default(); + } + } +} + +fn to_surface_vibration_coeff(surface: SurfaceTypeMsfs) -> f64 { + match surface { + SurfaceTypeMsfs::Concrete => 1., + SurfaceTypeMsfs::Grass => 10., + SurfaceTypeMsfs::Dirt => 10., + SurfaceTypeMsfs::GrassBumpy => 15., + SurfaceTypeMsfs::Bituminus => 2., + SurfaceTypeMsfs::Tarmac => 1.2, + SurfaceTypeMsfs::Asphalt => 1., + SurfaceTypeMsfs::Macadam => 1.5, + _ => 3., + } +} + +pub struct SurfaceVibrationGenerator { + bumps: Vec, + + final_bump_accel: Acceleration, + + final_bump_accel_filtered: LowPassFilter, +} +impl SurfaceVibrationGenerator { + pub fn default() -> Self { + let big_holes = BumpGenerator::new(Duration::from_secs(25), 2., 0.4); + let small_holes = BumpGenerator::new(Duration::from_secs(5), 1.1, 0.1); + let vibrations = BumpGenerator::new(Duration::from_millis(20), 0.2, 0.05); + + let all_bumps = vec![vibrations, small_holes, big_holes]; + + Self { + bumps: all_bumps, + final_bump_accel: Acceleration::default(), + + final_bump_accel_filtered: LowPassFilter::new(Duration::from_millis(30)), + } + } + + pub fn update(&mut self, context: &UpdateContext, weight_on_wheels_ratio: Ratio) { + self.final_bump_accel = Acceleration::default(); + + let local_velocity = + Velocity::new::(context.local_velocity().to_ms_vector()[2]); + + let velocity_coeff = if local_velocity.get::() < 2. { + 0. + } else { + 0.1 * local_velocity.get::().abs().sqrt() + }; + + for bump in &mut self.bumps { + bump.update(context); + self.final_bump_accel += bump.bump_accel + * weight_on_wheels_ratio + * to_surface_vibration_coeff(context.surface_type()) + * velocity_coeff; + } + + self.final_bump_accel_filtered + .update(context.delta(), self.final_bump_accel); + } + + pub fn surface_vibration_acceleration(&self) -> Acceleration { + self.final_bump_accel_filtered.output() + } +} diff --git a/fbw-common/src/wasm/systems/systems/src/structural_flex/wing_flex.rs b/fbw-common/src/wasm/systems/systems/src/structural_flex/wing_flex.rs new file mode 100644 index 00000000000..4658ed1eac3 --- /dev/null +++ b/fbw-common/src/wasm/systems/systems/src/structural_flex/wing_flex.rs @@ -0,0 +1,688 @@ +use crate::shared::local_acceleration_at_plane_coordinate; +use crate::shared::low_pass_filter::LowPassFilter; +use crate::shared::update_iterator::MaxStepLoop; + +use crate::simulation::{ + InitContext, Read, SimulationElement, SimulationElementVisitor, SimulatorReader, UpdateContext, + VariableIdentifier, +}; + +use uom::si::{ + acceleration::meter_per_second_squared, + angle::radian, + f64::*, + force::newton, + length::meter, + mass::kilogram, + ratio::percent, + ratio::ratio, + velocity::{knot, meter_per_second}, +}; + +use std::time::Duration; + +use nalgebra::{Vector2, Vector3}; + +enum GearStrutId { + Nose = 0, + LeftBody = 1, + RightBody = 2, + LeftWing = 3, + RightWing = 4, +} +struct LandingGearWeightOnWheelsEstimator { + center_compression_id: VariableIdentifier, + + left_wing_compression_id: VariableIdentifier, + right_wing_compression_id: VariableIdentifier, + + left_body_compression_id: VariableIdentifier, + right_body_compression_id: VariableIdentifier, + + center_compression: Ratio, + left_wing_compression: Ratio, + right_wing_compression: Ratio, + left_body_compression: Ratio, + right_body_compression: Ratio, +} +impl LandingGearWeightOnWheelsEstimator { + const GEAR_CENTER_COMPRESSION: &'static str = "CONTACT POINT COMPRESSION:0"; + const GEAR_LEFT_BODY_COMPRESSION: &'static str = "CONTACT POINT COMPRESSION:1"; + const GEAR_RIGHT_BODY_COMPRESSION: &'static str = "CONTACT POINT COMPRESSION:2"; + const GEAR_LEFT_WING_COMPRESSION: &'static str = "CONTACT POINT COMPRESSION:3"; + const GEAR_RIGHT_WING_COMPRESSION: &'static str = "CONTACT POINT COMPRESSION:4"; + + // Weight estimation is in the form of weight = X * compression_percent^(Y) + const NOSE_GEAR_X_COEFF: f64 = 2.22966; + const NOSE_GEAR_Y_POW: f64 = 2.2953179884; + + const WING_GEAR_X_COEFF: f64 = 5.5; + const WING_GEAR_Y_POW: f64 = 2.6; + + const BODY_GEAR_X_COEFF: f64 = 7.5; + const BODY_GEAR_Y_POW: f64 = 2.5; + + fn new(context: &mut InitContext) -> Self { + Self { + center_compression_id: context.get_identifier(Self::GEAR_CENTER_COMPRESSION.to_owned()), + left_wing_compression_id: context + .get_identifier(Self::GEAR_LEFT_WING_COMPRESSION.to_owned()), + right_wing_compression_id: context + .get_identifier(Self::GEAR_RIGHT_WING_COMPRESSION.to_owned()), + left_body_compression_id: context + .get_identifier(Self::GEAR_LEFT_BODY_COMPRESSION.to_owned()), + right_body_compression_id: context + .get_identifier(Self::GEAR_RIGHT_BODY_COMPRESSION.to_owned()), + + center_compression: Ratio::default(), + left_wing_compression: Ratio::default(), + right_wing_compression: Ratio::default(), + left_body_compression: Ratio::default(), + right_body_compression: Ratio::default(), + } + } + + fn total_weight_on_wheels(&self) -> Mass { + self.weight_on_wheel(GearStrutId::Nose) + + self.weight_on_wheel(GearStrutId::LeftWing) + + self.weight_on_wheel(GearStrutId::RightWing) + + self.weight_on_wheel(GearStrutId::LeftBody) + + self.weight_on_wheel(GearStrutId::RightBody) + } + + fn weight_on_wheel(&self, wheel_id: GearStrutId) -> Mass { + let (coeff, compression, exponent) = match wheel_id { + GearStrutId::Nose => ( + Self::NOSE_GEAR_X_COEFF, + self.center_compression, + Self::NOSE_GEAR_Y_POW, + ), + GearStrutId::LeftWing => ( + Self::WING_GEAR_X_COEFF, + self.left_wing_compression, + Self::WING_GEAR_Y_POW, + ), + GearStrutId::RightWing => ( + Self::WING_GEAR_X_COEFF, + self.right_wing_compression, + Self::WING_GEAR_Y_POW, + ), + GearStrutId::LeftBody => ( + Self::BODY_GEAR_X_COEFF, + self.left_body_compression, + Self::BODY_GEAR_Y_POW, + ), + GearStrutId::RightBody => ( + Self::BODY_GEAR_X_COEFF, + self.right_body_compression, + Self::BODY_GEAR_Y_POW, + ), + }; + + Mass::new::(coeff * compression.get::().powf(exponent)) + } +} +impl SimulationElement for LandingGearWeightOnWheelsEstimator { + fn read(&mut self, reader: &mut SimulatorReader) { + self.center_compression = reader.read(&self.center_compression_id); + self.left_wing_compression = reader.read(&self.left_body_compression_id); + self.right_wing_compression = reader.read(&self.right_body_compression_id); + self.left_body_compression = reader.read(&self.left_wing_compression_id); + self.right_body_compression = reader.read(&self.right_wing_compression_id); + } +} + +// Computes a global lift force from anything we can use from the sim +pub struct WingLift { + gear_weight_on_wheels: LandingGearWeightOnWheelsEstimator, + + total_lift: Force, + + ground_weight_ratio: Ratio, +} +impl WingLift { + pub fn new(context: &mut InitContext) -> Self { + Self { + gear_weight_on_wheels: LandingGearWeightOnWheelsEstimator::new(context), + total_lift: Force::default(), + ground_weight_ratio: Ratio::default(), + } + } + + pub fn update(&mut self, context: &UpdateContext) { + let total_weight_on_wheels = self.gear_weight_on_wheels.total_weight_on_wheels(); + + let raw_accel_no_grav = context.vert_accel().get::(); + let cur_weight_kg = context.total_weight().get::(); + + let lift_delta_from_accel_n = raw_accel_no_grav * cur_weight_kg; + let lift_1g = 9.8 * cur_weight_kg; + let lift_wow = -9.8 * total_weight_on_wheels.get::(); + + let lift = if total_weight_on_wheels.get::() > 500. { + // Assuming no lift at low wind speed avoids glitches with ground when braking hard for a full stop + if context.true_airspeed().get::().abs() < 20. { + 0. + } else { + (lift_1g + lift_wow).max(0.) + } + } else { + lift_1g + lift_delta_from_accel_n + }; + + self.total_lift = Force::new::(lift); + + self.ground_weight_ratio = total_weight_on_wheels / context.total_weight(); + } + + pub fn total_plane_lift(&self) -> Force { + self.total_lift + } + + // Outputs the fraction of the weight of the plane that is applied on ground. + // 0-> Plane not on ground 0.5-> half the weight of the plane on ground ... + pub fn ground_weight_ratio(&self) -> Ratio { + Ratio::new::(self.ground_weight_ratio.get::().clamp(0., 1.)) + } + + pub fn total_lift(&self) -> Force { + self.total_lift + } +} +impl SimulationElement for WingLift { + fn accept(&mut self, visitor: &mut T) { + self.gear_weight_on_wheels.accept(visitor); + + visitor.visit(self); + } +} + +// Map that gives the mass of each wingflex node given the masses of the plane fuel tanks +pub struct WingFuelNodeMapper { + fuel_tank_mapping: [usize; FUEL_TANK_NUMBER], +} +impl + WingFuelNodeMapper +{ + // For each fuel tank, gives the index of the wing node where fuel mass is added + pub fn new(fuel_tank_mapping: [usize; FUEL_TANK_NUMBER]) -> Self { + Self { fuel_tank_mapping } + } + + pub fn fuel_masses(&self, fuel_tanks_masses: [Mass; FUEL_TANK_NUMBER]) -> [Mass; NODE_NUMBER] { + let mut masses = [Mass::default(); NODE_NUMBER]; + for (idx, fuel) in fuel_tanks_masses.iter().enumerate() { + masses[self.fuel_tank_mapping[idx]] += *fuel; + } + masses + } +} + +/// Computes the vertical acceleration in plane local Y coordinate of the root of a wing +/// Takes the points coordinates of the root of the wing relative to datum point in the model +pub struct WingRootAcceleration { + wing_root_position_meters: Vector3, + + total_wing_root_accel_filtered: LowPassFilter, +} +impl WingRootAcceleration { + pub fn new(wing_root_position_meters: Vector3) -> Self { + Self { + wing_root_position_meters, + + total_wing_root_accel_filtered: LowPassFilter::new(Duration::from_millis(1)), + } + } + + pub fn update(&mut self, context: &UpdateContext) { + let local_wing_root_accel = + local_acceleration_at_plane_coordinate(context, self.wing_root_position_meters); + + let total_wing_root_accel = + context.vert_accel().get::() + local_wing_root_accel[1]; + + self.total_wing_root_accel_filtered + .update(context.delta(), total_wing_root_accel); + } + + pub fn acceleration(&self) -> Acceleration { + Acceleration::new::(self.total_wing_root_accel_filtered.output()) + } +} + +/// A flexible constraint with elasticity and damping property. Represent the bending flex force between two wing nodes +struct FlexibleConstraint { + springiness: f64, + damping: f64, + + negative_springiness_coeff: f64, + is_linear: bool, + + previous_length: Length, + damping_force: LowPassFilter, + + total_force: Force, +} +impl FlexibleConstraint { + // Damping is low pass filtered which results in improved numerical stability + const DAMPING_FILTERING_TIME_CONSTANT: Duration = Duration::from_millis(10); + + fn new( + springiness: f64, + damping: f64, + is_linear: bool, + negative_springiness_coeff: Option, + ) -> Self { + Self { + springiness, + damping, + negative_springiness_coeff: if let Some(coeff) = negative_springiness_coeff { + coeff + } else { + 1. + }, + is_linear, + previous_length: Length::default(), + damping_force: LowPassFilter::new(Self::DAMPING_FILTERING_TIME_CONSTANT), + total_force: Force::default(), + } + } + + fn update( + &mut self, + context: &UpdateContext, + nodes: &mut [WingSectionNode], //nodes[0] wing root side node nodes[1] wing tip side node + ) { + let length = nodes[1].position() - nodes[0].position(); + + let spring_force = if length.get::() < 0. { + if self.is_linear { + Force::new::( + length.get::() * self.springiness * self.negative_springiness_coeff, + ) + } else { + -Force::new::( + (-length.get::()).exp() * self.springiness - self.springiness, + ) * self.negative_springiness_coeff + } + } else if self.is_linear { + Force::new::(length.get::() * self.springiness) + } else { + Force::new::(length.get::().exp() * self.springiness - self.springiness) + }; + + let speed = (self.previous_length - length) / context.delta_as_time(); + + let raw_damping_force = + Force::new::(-speed.get::() * self.damping); + + self.damping_force + .update(context.delta(), raw_damping_force); + + self.total_force = spring_force + self.damping_force.output(); + + self.previous_length = length; + + // Spring force is computed, we apply it back to left and right nodes: same but opposite force + nodes[0].apply_force(self.total_force); + nodes[1].apply_force(-self.total_force); + } +} + +/// A wing node with a empty mass and a fuel mass. Can be connected to another node through a flexible constraint +struct WingSectionNode { + empty_mass: Mass, + fuel_mass: Mass, + speed: Velocity, + position: Length, + acceleration: Acceleration, + + external_position_offset: Length, + + sum_of_forces: Force, +} +impl WingSectionNode { + fn new(empty_mass: Mass) -> Self { + Self { + empty_mass, + fuel_mass: Mass::default(), + speed: Velocity::default(), + position: Length::default(), + acceleration: Acceleration::default(), + + external_position_offset: Length::default(), + + sum_of_forces: Force::default(), + } + } + + fn update(&mut self, context: &UpdateContext) { + self.apply_gravity_force(context); + self.solve_physics(context); + } + + fn gravity_on_plane_y_axis(context: &UpdateContext) -> Acceleration { + let pitch_rotation = context.attitude().pitch_rotation_transform(); + + let bank_rotation = context.attitude().bank_rotation_transform(); + + let gravity_acceleration_world_reference = Vector3::new(0., -9.8, 0.); + + // Total acceleration in plane reference is the gravity in world reference rotated to plane reference. + let local_gravity_plane_reference = + pitch_rotation * (bank_rotation * gravity_acceleration_world_reference); + + Acceleration::new::(local_gravity_plane_reference[1]) + } + + fn apply_gravity_force(&mut self, context: &UpdateContext) { + self.sum_of_forces += Force::new::( + Self::gravity_on_plane_y_axis(context).get::() + * self.total_mass().get::(), + ); + } + + fn solve_physics(&mut self, context: &UpdateContext) { + if self.empty_mass.get::() > 0. { + self.acceleration = self.sum_of_forces / self.total_mass(); + + self.speed += self.acceleration * context.delta_as_time(); + + self.position += self.speed * context.delta_as_time(); + } + + self.sum_of_forces = Force::default(); + } + + fn total_mass(&self) -> Mass { + self.empty_mass + self.fuel_mass + } + + fn apply_force(&mut self, force: Force) { + self.sum_of_forces += force; + } + + fn apply_external_offet(&mut self, offset: Length) { + self.external_position_offset = offset; + } + + fn position(&self) -> Length { + self.position + self.external_position_offset + } + + fn acceleration(&self) -> Acceleration { + self.acceleration + } + + fn set_fuel_mass(&mut self, fuel_mass: Mass) { + self.fuel_mass = fuel_mass; + } + + fn fuel_mass(&self) -> Mass { + self.fuel_mass + } +} + +pub struct FlexPhysicsNG { + updater_max_step: MaxStepLoop, + + nodes: [WingSectionNode; NODE_NUMBER], + flex_constraints: [FlexibleConstraint; LINK_NUMBER], + + // DEV simvars to adjust parameters ingame + wing_dev_spring_1_id: VariableIdentifier, + wing_dev_spring_2_id: VariableIdentifier, + wing_dev_spring_3_id: VariableIdentifier, + wing_dev_spring_4_id: VariableIdentifier, + + wing_dev_damping_1_id: VariableIdentifier, + wing_dev_damping_2_id: VariableIdentifier, + wing_dev_damping_3_id: VariableIdentifier, + wing_dev_damping_4_id: VariableIdentifier, + + neg_flex_coeff_id: VariableIdentifier, + exponent_flex_id: VariableIdentifier, + + external_accelerations_filtered: LowPassFilter, +} +impl FlexPhysicsNG { + const MIN_PHYSICS_SOLVER_TIME_STEP: Duration = Duration::from_millis(5); + + // Limits max impulse wing can receive from the plane as MSFS could send huge impulses when craching the plane + const MAX_G_FORCE_IMPACT_APPLIED_ON_WING_ROOT_BY_PLANE_M_S2: f64 = 150.; + + // Plane accelerations are comunicated to the wing by artificially move the root node proportionaly to acceleration using this gain + // More gain will cause wing being more sensitive to plane Y accelerations + // Negative because if plane suddenly goes up, it locally throws wing down in wing frame of reference + const PLANE_ACCEL_TO_WING_ROOT_OFFSET_GAIN: f64 = -0.006; + + pub fn new( + context: &mut InitContext, + empty_mass: [Mass; NODE_NUMBER], + springness: [f64; LINK_NUMBER], + damping: [f64; LINK_NUMBER], + ) -> Self { + let nodes_array = empty_mass.map(WingSectionNode::new); + + let links_array = springness + .iter() + .zip(damping) + .map(|(springness, damping)| { + FlexibleConstraint::new(*springness, damping, false, Some(1.4)) + }) + .collect::>(); + Self { + updater_max_step: MaxStepLoop::new(Self::MIN_PHYSICS_SOLVER_TIME_STEP), + + nodes: nodes_array, + flex_constraints: links_array.try_into().unwrap_or_else( + |v: Vec| { + panic!( + "Expected a Vec of length {} but it was {}", + LINK_NUMBER, + v.len() + ) + }, + ), + + wing_dev_spring_1_id: context.get_identifier("WING_FLEX_DEV_SPRING_1".to_owned()), + wing_dev_spring_2_id: context.get_identifier("WING_FLEX_DEV_SPRING_2".to_owned()), + wing_dev_spring_3_id: context.get_identifier("WING_FLEX_DEV_SPRING_3".to_owned()), + wing_dev_spring_4_id: context.get_identifier("WING_FLEX_DEV_SPRING_4".to_owned()), + + wing_dev_damping_1_id: context.get_identifier("WING_FLEX_DEV_DAMPING_1".to_owned()), + wing_dev_damping_2_id: context.get_identifier("WING_FLEX_DEV_DAMPING_2".to_owned()), + wing_dev_damping_3_id: context.get_identifier("WING_FLEX_DEV_DAMPING_3".to_owned()), + wing_dev_damping_4_id: context.get_identifier("WING_FLEX_DEV_DAMPING_4".to_owned()), + neg_flex_coeff_id: context.get_identifier("WING_FLEX_DEV_NEG_STIFF_COEFF".to_owned()), + exponent_flex_id: context.get_identifier("WING_FLEX_DEV_STIFF_EXPO_ENA".to_owned()), + + external_accelerations_filtered: LowPassFilter::new(Duration::from_millis(50)), + } + } + + pub fn update( + &mut self, + context: &UpdateContext, + lift_forces: &[f64], + fuel_masses: [Mass; NODE_NUMBER], + external_acceleration_from_plane_body: Acceleration, + ) { + self.updater_max_step.update(context); + + for cur_time_step in &mut self.updater_max_step { + self.external_accelerations_filtered + .update(context.delta(), external_acceleration_from_plane_body); + + // Here we artificially move up or down wing root point so that plane movement is communicated to the rest of the wing + // through the flex constraints + self.nodes[0].apply_external_offet( + Self::PLANE_ACCEL_TO_WING_ROOT_OFFSET_GAIN + * Length::new::( + self.external_accelerations_filtered + .output() + .get::() + .clamp( + -Self::MAX_G_FORCE_IMPACT_APPLIED_ON_WING_ROOT_BY_PLANE_M_S2, + Self::MAX_G_FORCE_IMPACT_APPLIED_ON_WING_ROOT_BY_PLANE_M_S2, + ), + ), + ); + + // Solving flex physics of n-1 first nodes (N nodes N-1 Links) + for idx in 0..LINK_NUMBER { + self.nodes[idx].set_fuel_mass(fuel_masses[idx]); + + self.nodes[idx].apply_force(Force::new::(lift_forces[idx])); + + self.nodes[idx].update(&context.with_delta(cur_time_step)); + + self.flex_constraints[idx].update( + &context.with_delta(cur_time_step), + &mut self.nodes[idx..=idx + 1], + ); + } + + // Don't forget last node to solve as for loop solves only up to n-1 node + self.nodes[NODE_NUMBER - 1].set_fuel_mass(fuel_masses[NODE_NUMBER - 1]); + self.nodes[NODE_NUMBER - 1] + .apply_force(Force::new::(lift_forces[NODE_NUMBER - 1])); + self.nodes[NODE_NUMBER - 1].update(&context.with_delta(cur_time_step)); + } + } + + pub fn nodes_height_meters(&self) -> [f64; NODE_NUMBER] { + let mut all_heights_meters = [0.; NODE_NUMBER]; + + for (height, node) in all_heights_meters[1..].iter_mut().zip(&self.nodes[1..]) { + *height = node.position().get::(); + } + all_heights_meters + } + + pub fn acceleration_at_node_idx(&self, node_idx: usize) -> Acceleration { + assert!(node_idx < NODE_NUMBER); + + self.nodes[node_idx].acceleration() + } + + pub fn node_fuel_mass(&self, node_id: usize) -> Mass { + self.nodes[node_id].fuel_mass() + } +} +impl SimulationElement for FlexPhysicsNG<5, 4> { + fn read(&mut self, reader: &mut SimulatorReader) { + let node1_spring = reader.read(&self.wing_dev_spring_1_id); + if node1_spring > 0. { + self.flex_constraints[0].springiness = node1_spring; + } + + let node2_spring = reader.read(&self.wing_dev_spring_2_id); + if node2_spring > 0. { + self.flex_constraints[1].springiness = node2_spring; + } + + let node3_spring = reader.read(&self.wing_dev_spring_3_id); + if node3_spring > 0. { + self.flex_constraints[2].springiness = node3_spring; + } + + let node4_spring = reader.read(&self.wing_dev_spring_4_id); + if node4_spring > 0. { + self.flex_constraints[3].springiness = node4_spring; + } + + let node1_damp = reader.read(&self.wing_dev_damping_1_id); + if node1_damp > 0. { + self.flex_constraints[0].damping = node1_damp; + } + + let node2_damp = reader.read(&self.wing_dev_damping_2_id); + if node2_damp > 0. { + self.flex_constraints[1].damping = node2_damp; + } + + let node3_damp = reader.read(&self.wing_dev_damping_3_id); + if node3_damp > 0. { + self.flex_constraints[2].damping = node3_damp; + } + + let node4_damp = reader.read(&self.wing_dev_damping_4_id); + if node4_damp > 0. { + self.flex_constraints[3].damping = node4_damp; + } + + let neg_coeff = reader.read(&self.neg_flex_coeff_id); + if neg_coeff > 0. { + self.flex_constraints[0].negative_springiness_coeff = neg_coeff; + self.flex_constraints[1].negative_springiness_coeff = neg_coeff; + self.flex_constraints[2].negative_springiness_coeff = neg_coeff; + self.flex_constraints[3].negative_springiness_coeff = neg_coeff; + } else { + self.flex_constraints[0].negative_springiness_coeff = 1.; + self.flex_constraints[1].negative_springiness_coeff = 1.; + self.flex_constraints[2].negative_springiness_coeff = 1.; + self.flex_constraints[3].negative_springiness_coeff = 1.; + } + + let is_expo: f64 = reader.read(&self.exponent_flex_id); + if is_expo > 0.1 { + self.flex_constraints[0].is_linear = false; + self.flex_constraints[1].is_linear = false; + self.flex_constraints[2].is_linear = false; + self.flex_constraints[3].is_linear = false; + } else { + self.flex_constraints[0].is_linear = true; + self.flex_constraints[1].is_linear = true; + self.flex_constraints[2].is_linear = true; + self.flex_constraints[3].is_linear = true; + } + } +} + +// Takes height of each node and returns the angles between last nodes from wing root to tip +// This is used because animation bones are parent/child from root to tip +pub struct WingAnimationMapper { + x_positions: [f64; NODE_NUMBER], +} +impl WingAnimationMapper { + pub fn new(x_positions: [f64; NODE_NUMBER]) -> Self { + Self { x_positions } + } + + pub fn animation_angles(&self, wing_node_heights: [f64; NODE_NUMBER]) -> [Angle; NODE_NUMBER] { + let mut animation_angles = [Angle::default(); NODE_NUMBER]; + + let mut previous_node_coord = Vector2::new(1., 0.); + + for idx in 1..NODE_NUMBER { + let cur_node_coord = Vector2::new( + self.x_positions[idx] - self.x_positions[idx - 1], + wing_node_heights[idx], + ); + let dot_prod = previous_node_coord + .normalize() + .dot(&cur_node_coord.normalize()); + + animation_angles[idx] = + if Self::is_positive_angle(&previous_node_coord, &cur_node_coord) { + Angle::new::(dot_prod.acos()) + } else { + -Angle::new::(dot_prod.acos()) + }; + + previous_node_coord = cur_node_coord; + } + + animation_angles + } + + fn is_positive_angle(v1: &Vector2, v2: &Vector2) -> bool { + Self::cross(v1, v2) >= 0. + } + + fn cross(v1: &Vector2, v2: &Vector2) -> f64 { + (v1[0] * v2[1]) - (v1[1] * v2[0]) + } +} +impl SimulationElement for WingAnimationMapper<5> {}