Tag: variable

I’m currently trying to port a flight dynamics model that I’ve implemented in Rust to Unity. The original FDM utilized the Hecs ECS, and I plan on implementing this same model in Unity using the ECS framework provided by the engine.

The main issue I keep running into is how to access state data (a struct containing locations, velocities, and various other components like bank angle, throttle and flap) associated with my aircraft entity. In the rust version of this model, my main function begins by instantiating this state struct (among other variables) with the necessary starting values, and continues into an infinite loop (the “game” loop, so to speak) in which keyboard states are checked, the state struct is updated according to the various inputs, and the entity is updated with Rust’s equivalent of a “ForEach” function.

However, when I create a system in Unity, the equivalent of the game loop seems to be the “OnUpdate” function. This becomes an issue because I need to be able to instantiate (and access) the state struct outside of the “ForEach” function. I thought about placing the instantiation of the state struct inside of a “Start” function, but that would require some sort of variable local to the class (which, from what I recall, is not ideal when creating a system, which shouldn’t contain data, but rather the logic).

I guess what I’m trying to ask is this: is it a good idea (or even possible within a class that uses SystemBase, for that matter) to create a variable within the class itself (and not within a function, for instance) that can be used by both my “Start” and “OnUpdate” functions? If not, is there a workaround for this? Or, since the state struct is not attached to the actual entity (since the only data associated with the aircraft within the world is its translation data), and only serves to help in the helper functions for the equations of motion, would it be acceptable?

Thanks in advance! I’ll include the code snippets of the rust implementation below. If I omitted anything important, I apologize. I’ve been trying to figure out how to explain this problem as concisely as I can, so I may have missed some important detail.

use std::{process, thread, time::Duration};
use gphys::palmer::fdm::{Properties, State, eom};
use hecs::*;
use console::Term;
use device_query::{DeviceQuery, DeviceState, Keycode};

#(derive(Debug))
struct Position {
    x: i32,
    y: i32,
    z: i32
}

#(derive(Debug))
struct Velocity {
    vx: i32,
    vy: i32,
    vz: i32
}

fn check_ranges(state: &mut State) {
    // throttle range bounds enforcement
    if state.throttle > 1.0  { state.throttle = 1.0; }
    if state.throttle < 0.0  { state.throttle = 0.0; }
    // angle of attack bounds enforcement
    if state.alpha > 20.0    { state.alpha = 20.0;   }
    if state.alpha < 0.0     { state.alpha = 0.0;    }
    // bank/roll angle bounds enforcement
    if state.bank > 20.0     { state.bank = 20.0;    }
    if state.bank < -20.0    { state.bank = -20.0;   }
    // flap deflection bounds enforcement
    if state.flap > 40.0     { state.flap = 40.0;    }
    if state.flap < 0.0      { state.flap = 0.0;     }
}

#(allow(dead_code))
fn print_state(term: &Term, state: &State) {
    // velocity information
    let vx = state.q(0);
    let vy = state.q(2);
    let vz = state.q(4);
    // position information
    //let x = state.q(1);
    //let y = state.q(3);
    let z = state.q(5);

    let heading = vy.atan2(vx).to_degrees();
    let vh: f64 = (vx * vx + vy * vy).sqrt();
    let climb_angle = (vz / vh).atan().to_degrees();
    let air_speed = (vx * vx + vy * vy + vz * vz).sqrt();

    let s = format!("Throttle: {:>3.1}% | Angle of Attack: {:>3.1} deg | Bank Angle: {:>3.1} deg | Flap Deflection: {:>3.1} deg | Heading: {:>3.1} deg | Climb Angle: {:>3.1} deg | Air Speed: {:>3.1} | Climb Rate: {:>3.1} | Altitude: {:>3.1}",
                    (state.throttle * 100.0).round(),
                    state.alpha,
                    state.bank, 
                    state.flap,
                    heading,
                    climb_angle,
                    air_speed,
                    vz,
                    z);

    term.clear_line().unwrap();
    term.write_str(&s).unwrap();
}

fn spawn_entity(world: &mut World, state: &State) {
    let pos = Position {
        x: state.q(1) as i32,
        y: state.q(3) as i32,
        z: state.q(5) as i32
    };
    let velocity = Velocity {
        vx: state.q(0) as i32,
        vy: state.q(2) as i32,
        vz: state.q(4) as i32
    };
    world.spawn((pos, velocity));
}

fn update_entity(world: &mut World, state: &State) {
    for (_id, (pos, velocity)) in &mut world.query::<(&mut Position, &mut Velocity)>() {
        // Update positions
        pos.x = state.q(1) as i32;
        pos.y = state.q(3) as i32;
        pos.z = state.q(5) as i32;
        // Update velocities
        velocity.vx = state.q(0) as i32;
        velocity.vy = state.q(2) as i32;
        velocity.vz = state.q(4) as i32;
    }
}

#(allow(dead_code))
fn print_entity_debug_tool(term: &Term, world: &mut World) {
    // Function I used when testing this file to make sure that the entity is being written to properly.
    // Uncomment this function wherever it appears in the main loop to see the results.
    // Also recommended to comment out the fdm console term output to better see these results.
    // Doing so might generate a few "function is never used" errors for the omitted functions.
    for (id, (pos, velocity)) in &mut world.query::<(&Position, &Velocity)>() {
        let s = format!("Entity ID: {:?} ||  {:?} ||  {:?}",
                        id,
                        pos,
                        velocity);
        term.clear_line().unwrap();
        term.write_str(&s).unwrap();
    }
}

fn main() {
    // Cessna 172 properties
    let prop = Properties {
        wing_area: 16.2,
        wing_span: 10.9,
        tail_area: 2.0,
        cl_slope0: 0.0889,      // slope of Cl-alpha curve
        cl0: 0.178,             // intercept of Cl-alpha curve
        cl_slope1: -0.1,        // post-stall slope of Cl-alpha curve
        cl1: 3.2,               // post-stall intercept of Cl-alpha curve
        alpha_cl_max: 16.0,     // alpha when Cl=Clmax
        cdp: 0.034,             // parasite drag coefficient
        eff: 0.77,              // induced drag efficiency coefficient
        mass: 1114.0,
        engine_power: 119310.0,
        engine_rps: 40.0,       // revolutions per second
        prop_diameter: 1.905,
        a: 1.83,                //  propeller efficiency coefficient
        b: -1.32,               //  propeller efficiency coefficient
    };

    // simulation data (inputs/outputs)
    let mut state = State {
        time: 0.0,         // time
        q: (0.0; 6),       // ODE results
        bank: 0.0,         // roll angle
        alpha: 4.0,        // pitch angle
        throttle: 0.0,     // throttle percentage
        flap: 0.0,         // flap deflection
    };

    let mut world = World::new();
    spawn_entity(&mut world, &state);

    let device_state = DeviceState::new();
    let mut prev_keys = vec!();
let term = Term::stdout();
const FRAME_RATE : u64 = 60;           // hertz
const DT: f64 = 1.0/FRAME_RATE as f64; // seconds
loop {
    // compute equations of motion
    eom(&prop, &mut state, DT);
    // Comment out this print function if "print_entity_debug_tool" is uncommented below for better readability
    print_state(&term, &state);

    // Update entity after eom
    update_entity(&mut world, &state);

    // Considered adding an "update_entity()" function call here, but figured just one call after the range check should be necessary

    // update state
    let keys = device_state.get_keys();
    if keys != prev_keys {
        if keys.contains(&Keycode::E) {
            state.throttle += 0.1;
        } else if keys.contains(&Keycode::D) {
            state.throttle -= 0.1;
        } else if keys.contains(&Keycode::Up) {
            state.alpha += 1.0;
        } else if keys.contains(&Keycode::Down) {
            state.alpha -= 1.0;
        } else if keys.contains(&Keycode::Left) {
            state.bank -= 1.0;
        } else if keys.contains(&Keycode::Right) {
            state.bank += 1.0;
        } else if keys.contains(&Keycode::L) {
            state.flap -= 1.0;
        } else if keys.contains(&Keycode::K) {
            state.flap += 1.0;
        } else if keys.contains(&Keycode::Q) {
            process::exit(0);
        }
        check_ranges(&mut state);
    }
    // Update entity after range check to ensure entity is also within bounds
    update_entity(&mut world, &state);

    // Remove the commented-out function to show entity values being written to
    //print_entity_debug_tool(&term, &mut world);

    prev_keys = keys;
    thread::sleep(Duration::from_millis((DT*100.0) as u64));
}
}

And here’s the function containing the equation of motion helper functions:

use std::f64::consts::PI;
static G: f64 = -9.81;

#(derive(Debug, Default))
pub struct Properties
{
    pub wing_area: f64,
    pub wing_span: f64,
    pub tail_area: f64,
    pub cl_slope0: f64,      // slope of Cl-alpha curve
    pub cl0: f64,            // intercept of Cl-alpha curve
    pub cl_slope1: f64,      // post-stall slope of Cl-alpha curve
    pub cl1: f64,            // post-stall intercept of Cl-alpha curve
    pub alpha_cl_max: f64,   // alpha when Cl=Clmax
    pub cdp: f64,            // parasite drag coefficient
    pub eff: f64,            // induced drag efficiency coefficient
    pub mass: f64,
    pub engine_power: f64,
    pub engine_rps: f64,     // revolutions per second
    pub prop_diameter: f64,
    pub a: f64,              //  propeller efficiency coefficient
    pub b: f64,              //  propeller efficiency coefficient
}

#(derive(Debug, Default))
pub struct State
{
    pub time: f64,         // time
    pub q: (f64; 6),       // ODE results

    pub bank: f64,         // roll angle
    pub alpha: f64,        // pitch angle
    pub throttle: f64,     // throttle percentage
    pub flap: f64,         // flap deflection
}

//-----------------------------------------------------
// calculates the forces associated with an aircraft
// given a set of properties and current state
//-----------------------------------------------------
fn plane_rhs(prop: &Properties, state: &State,
             q: &(f64; 6), delta_q: &(f64; 6),
             dt: f64, q_scale: f64,
             dq: &mut (f64; 6))
{
    // property convenience variables
    let wing_area = prop.wing_area;
    let wing_span = prop.wing_span;
//    let tail_area = plane.tail_area;
    let cl_slope0 = prop.cl_slope0;
    let cl0 = prop.cl0;
    let cl_slope1 = prop.cl_slope1;
    let cl1 = prop.cl1;
    let alpha_cl_max = prop.alpha_cl_max;
    let cdp = prop.cdp;
    let eff = prop.eff;
    let mass = prop.mass;
    let engine_power = prop.engine_power;
    let engine_rps = prop.engine_rps;
    let prop_diameter = prop.prop_diameter;
    let a = prop.a;
    let b = prop.b;

    // state convenience variables
    let alpha = state.alpha;
    let throttle = state.throttle;
    let flap = state.flap;

    // convert bank angle from degrees to radians
    // angle of attack is not converted because the
    // Cl-alpha curve is defined in terms of degrees
    let bank = state.bank.to_radians();

    // compute the intermediate values of the dependent variables
    let mut new_q : (f64; 6) = (0.0; 6);
    for i in 0..6 {
        new_q(i) = q(i) + q_scale * delta_q(i); 
    }
    
    // assign convenenience variables to the intermediate
    // values of the locations and velocities
    let vx: f64 = new_q(0);
    let vy: f64 = new_q(2);
    let vz: f64 = new_q(4);
    let _x: f64 = new_q(1);
    let _y: f64 = new_q(3);
    let z: f64 = new_q(5);
    let vh: f64 = (vx * vx + vy * vy).sqrt();
    let vtotal: f64 = (vx * vx + vy * vy + vz * vz).sqrt();

    // compute the air density
    let temperature: f64 = 288.15 - 0.0065 * z;
    let grp: f64 = 1.0 - 0.0065 * z / 288.15;
    let pressure: f64 = 101325.0 * (grp.powf(5.25));
    let density: f64 = 0.00348 * pressure / temperature;

    // compute power drop-off factor
    let omega: f64 = density / 1.225;
    let factor: f64 = (omega - 0.12)/  0.88;

    // compute thrust 
    let advance_ratio: f64 = vtotal / (engine_rps * prop_diameter);
    let thrust: f64 = throttle * factor * engine_power * (a + b * advance_ratio * advance_ratio) / (engine_rps * prop_diameter);

    // compute lift coefficient - the Cl curve is modeled using two straight lines
    let mut cl: f64;
    if alpha < alpha_cl_max {
        cl = cl_slope0 * alpha + cl0;
    } else {
        cl = cl_slope1 * alpha + cl1;
    }

    // include effects of flaps and ground effects
    // -- ground effects are present if the plane is within 5 meters of the ground
    if flap == 20.0 {
        cl += 0.25;
    }
    if flap == 40.0 {
        cl += 0.5;
    }
    if z < 5.0 {
        cl += 0.25;
    }

    // compute lift
    let lift: f64 = 0.5 * cl * density * vtotal * vtotal * wing_area;

    // compute drag coefficient
    let aspect_ratio: f64 = wing_span * wing_span / wing_area;
    let cd = cdp + cl * cl / (PI * aspect_ratio * eff);

    // compute drag force
    let drag: f64 = 0.5 * cd * density * vtotal * vtotal * wing_area;

    // define some shorthand convenience variables for use with the rotation matrix
    // compute the sine and cosines of the climb angle, bank angle, and heading angle

    let cos_w: f64 = bank.cos(); 
    let sin_w: f64 = bank.sin(); 

    let cos_p: f64;   //  climb angle
    let sin_p: f64;   //  climb angle
    let cos_t: f64;   //  heading angle
    let sin_t: f64;   //  heading angle
    if vtotal == 0.0 {
        cos_p = 1.0;
        sin_p = 0.0;
    } else {
        cos_p = vh / vtotal;  
        sin_p = vz / vtotal;  
    }

    if vh == 0.0 {
        cos_t = 1.0;
        sin_t = 0.0;
    } else {
        cos_t = vx / vh;
        sin_t = vy / vh;
    }

    // convert the thrust, drag, and lift forces into x-, y-, and z-components using the rotation matrix
    let fx: f64 = cos_t * cos_p * (thrust - drag) + (sin_t * sin_w - cos_t * sin_p * cos_w) * lift;
    let fy: f64 = sin_t * cos_p * (thrust - drag) + (-cos_t * sin_w - sin_t * sin_p * cos_w) * lift;
    let mut fz: f64 = sin_p * (thrust - drag) + cos_p * cos_w * lift;

    // add the gravity force to the z-direction force.
    fz = fz + mass * G;

    // since the plane can't sink into the ground, if the altitude is less than or equal to zero and the z-component
    // of force is less than zero, set the z-force to be zero
    if z <= 0.0 && fz <= 0.0 {
        fz = 0.0;
    }

    // load the right-hand sides of the ODE's
    dq(0) = dt * (fx / mass);
    dq(1) = dt * vx;
    dq(2) = dt * (fy / mass);
    dq(3) = dt * vy;
    dq(4) = dt * (fz / mass);
    dq(5) = dt * vz;
}

//-----------------------------------------------------
// solves the equations of motion using the Runge-Kutta
// integration method
//-----------------------------------------------------
pub fn eom(prop: &Properties, state: &mut State, dt: f64)
{
    let mut q : (f64; 6) = (0.0; 6);
    let mut dq1 : (f64; 6) = (0.0; 6);
    let mut dq2 : (f64; 6) = (0.0; 6);
    let mut dq3 : (f64; 6) = (0.0; 6);
    let mut dq4 : (f64; 6) = (0.0; 6);

    // retrieve the current values of the dependent and independent variables
    for i in 0..6 {
        q(i) = state.q(i);
    }

    // compute the four Runge-Kutta steps, then return 
    // value of planeRightHandSide method is an array
    // of delta-q values for each of the four steps
    plane_rhs(&prop, &state, &q, &q, dt, 0.0, &mut dq1);
    plane_rhs(&prop, &state, &q, &dq1, dt, 0.5, &mut dq2);
    plane_rhs(&prop, &state, &q, &dq2, dt, 0.5, &mut dq3);
    plane_rhs(&prop, &state, &q, &dq3, dt, 1.0, &mut dq4);

    // update simulation time
    state.time += dt;

    // update the dependent and independent variable values
    // at the new dependent variable location and store the
    // values in the ODE object arrays
    for i in 0..6 {
        q(i) = q(i) + (dq1(i) + 2.0 * dq2(i) + 2.0 * dq3(i) + dq4(i)) / 6.0;
        state.q(i) = q(i);
    }
}