autopilot.cpp

#include "lander.h"
#include <vector>

double get_time_period(vector3d position, vector3d velocity) {
    // Calculate the time period of the orbit based on position and velocity - THIS WORKS. 
    // This function provides the max time the reentry computer has to consider, as it guarantees the lander will touch down within the next orbit.
    double mu = GRAVITY * (MARS_MASS + get_lander_mass(fuel)); // Standard gravitational parameter for Mars
    double semi_major_axis = (mu * position.abs()) / (2 * mu - velocity.abs2() * position.abs()); // Simplified estimate
    double period = 2 * M_PI * sqrt(semi_major_axis * semi_major_axis * semi_major_axis / mu);

    return period;
}


struct ApoapsiData {
    vector3d position;
    vector3d velocity;
    double time;
};

ApoapsiData apoapsis_computer(vector3d pos, vector3d vel, double f) {
    // This function calculates the trajectory of the lander and finds the apoapsis point - this is more efficient than running the computer twice
    // Returns the position, velocity, and time at apoapsis
    double t_max = simulation_time + get_time_period(pos, vel) * 1.5;
    double dt = delta_t;
    double computer_time = simulation_time; // start time at the current simulation time
    bool towards_periapsis = false; // Flag to indicate we are moving towards periapsis

    double computer_fuel = f;
    vector3d computer_position = pos;
    vector3d computer_velocity = vel;
    parachute_status_t computer_parachute_status = NOT_DEPLOYED; // current parachute status


    // First step
    vector3d computer_poslast = computer_position;
    computer_position += computer_velocity * delta_t;



    // Verlet integration
    for (computer_time = computer_time + dt; computer_time <= t_max; computer_time = computer_time + dt) {
        // Calculate new position and velocity

        // Calculate acceleration
        vector3d acceleration = vector3d(0.0, 0.0, 0.0);
        acceleration += -gravity(computer_position) * computer_position.norm() / get_lander_mass(computer_fuel); // Gravitational acceleration
        acceleration += -drag(computer_velocity, computer_position, computer_parachute_status) * computer_velocity.norm() / get_lander_mass(computer_fuel); // Drag acceleration

        // Verlet integration step
        vector3d new_position = 2 * computer_position - computer_poslast + dt * dt * acceleration;
        vector3d new_velocity = (new_position - computer_poslast) / (2.0 * dt); // Estimate velocity


        if (new_position.abs() > computer_position.abs()) {
            // If new position is further from Mars, that means we are moving towards periapsis. This should trigger a flag, allowing us to break the loop 
            towards_periapsis = true; // Flag to indicate we are moving towards periapsis

        }
        if (new_position.abs() < computer_position.abs() && towards_periapsis == true) {
            // If new position is closer to Mars, we have reached apoapsis
            break; // Exit the loop as we have found the apoapsis
        }
        computer_poslast = computer_position;
        computer_position = new_position;
        computer_velocity = new_velocity; // Update velocity for next iteration
    }
    // Return apoapsis data by grabbing the last position, velocity, and time before the loop breaks
    ApoapsiData apoapsis_data;
    apoapsis_data.position = computer_position;
    apoapsis_data.velocity = computer_velocity;
    apoapsis_data.time = computer_time;

    return apoapsis_data;
}

// Calculate orbital injection parameters
double get_injection_deltaV(vector3d pos, vector3d vel) {
    double current_altitude = pos.abs() - MARS_RADIUS;
    double mu = GRAVITY * MARS_MASS;

    // If already above target orbit, circularize at current altitude
    double injection_radius = max(pos.abs(), target_orbit_radius);

    // Calculate velocity needed for circular orbit at injection radius
    double circular_velocity = sqrt(mu / injection_radius);

    // Current velocity magnitude
    double current_velocity = vel.abs();

    // Calculate deltaV needed (assuming prograde burn)
    double deltaV = circular_velocity - current_velocity;

    return max(0.0, deltaV); // Don't allow negative deltaV
}

// Check if orbit is stable
bool is_orbit_stable(vector3d pos, vector3d vel) {
    double mu = GRAVITY * MARS_MASS;
    double current_radius = pos.abs();

    // Calculate orbital energy
    double kinetic_energy = 0.5 * vel.abs2();
    double potential_energy = -mu / current_radius;
    double total_energy = kinetic_energy + potential_energy;

    // For a stable orbit, total energy should be negative
    // and periapsis should be above the atmosphere
    if (total_energy >= 0) return false; // Hyperbolic trajectory

    // Calculate semi-major axis
    double semi_major_axis = -mu / (2.0 * total_energy);

    // Calculate eccentricity
    vector3d h = pos ^ vel; // Angular momentum vector
    double angular_momentum = h.abs();
    double eccentricity = sqrt(1.0 + (2.0 * total_energy * angular_momentum * angular_momentum) / (mu * mu));

    // Calculate periapsis
    double periapsis = semi_major_axis * (1.0 - eccentricity);

    // Orbit is stable if periapsis is above atmosphere (EXOSPHERE)
    return (periapsis > MARS_RADIUS + EXOSPHERE);
}

vector<double> reentry_computer(vector3d pos, vector3d vel, double f) {
    //This function is the simulator's "computer" that calculates the trajectory of the lander for the next period and a half.
    // It  called by the autopilot_initialize function to calculate the reentry deltaV and other parameters.
    //Returns the trajectory points as a vector of vector3d objects.
    double t_max = simulation_time + get_time_period(position, velocity) * 1.5; // 1.5 is a safety factor, to ensure the lander can touch down within the next orbit.
    double dt = delta_t;
    double computer_time = simulation_time; // start time at the current simulation time - need to be careful when this updates!

    double computer_fuel = f; // bad prac?
    vector3d computer_position = pos;
    vector3d computer_velocity = vel;
    parachute_status_t computer_parachute_status = NOT_DEPLOYED; // current parachute status




    vector <vector3d> trajectory; // This will store the trajectory points - only need to store position points
    trajectory.push_back(computer_position); // Add initial position to trajectory

    computer_position = computer_position + computer_velocity * dt; // first step
    trajectory.push_back(computer_position); // Add first step position to trajectory

    // Verlet integration
    for (computer_time = computer_time + dt; computer_time <= t_max; computer_time = computer_time + dt) {

        // calculate new position and velocity
        vector3d computer_poslast = trajectory.end()[-2];

        // this could maaaybe be done better, it is already being calculated in the main loop, but not for in the future.
        vector3d acceleration = vector3d(0.0, 0.0, 0.0);
        acceleration += -gravity(computer_position) * computer_position.norm() / get_lander_mass(computer_fuel); // Gravitational acceleration
        acceleration += -drag(computer_velocity, computer_position, computer_parachute_status) * computer_velocity.norm() / get_lander_mass(computer_fuel); // Drag acceleration


        computer_position = 2 * computer_position - computer_poslast + dt * dt * acceleration; // Verlet integration step
        // no need for velocity, as we only need the position for the trajectory

        // append new position to trajectory
        trajectory.push_back(computer_position);


    }

    // This could be improved by only storing the distances, but this is easier for now.

    //This function calculates the distances of the trajectory points from the center of Mars.
    vector<double> distances;

    for (const vector3d& point : trajectory) {
        distances.push_back(point.abs()); // Calculate distance from Mars center
    }
    return distances;

}

double get_reentry_deltaV(vector3d pos, vector3d vel, double f) {
    //this function uses the computer to calculate the reentry deltaV. It does this numerically by trying different deltaV values and checking if the lander can touch down within the next orbit
    //Only considers deltaV against the ground speed direction 
    double reentry_deltaV = 0.0; // Initial guess for reentry deltaV, can be adjusted based on requirements
    vector<double> zerodVcheck = reentry_computer(pos, vel, f); // Get the trajectory points with the current velocity, check if the lander can touch down without any deltaV adjustment
    double min_distance = *min_element(zerodVcheck.begin(), zerodVcheck.end());
    if (min_distance < MARS_RADIUS) {
        return reentry_deltaV = 0.0;
    }


    for (double deltaV = 100.0; deltaV < 10000.0; deltaV += 20.0) {

        cout << "Trying deltaV: " << deltaV << " m/s" << endl; // Debugging output to see the deltaV being tested 
        
        // Simulate the reentry burn by adjusting the velocity vector
        vector3d adjusted_velocity = vel - (deltaV * vel.norm()); // Adjust velocity by deltaV in the direction of the velocity vector
        // Call the reentry computer to get the predicted trajectory points with the adjusted velocity
        vector<double> predicted_distances = reentry_computer(pos, adjusted_velocity, f);
        // Check if the lander can touch down within the next orbit
        double min_distance = *min_element(predicted_distances.begin(), predicted_distances.end());
        if (min_distance < MARS_RADIUS) {
            reentry_deltaV = deltaV; // Found a valid deltaV that allows landing
            break;
        }
    }
    return reentry_deltaV;
}

double get_reentry_burn_fuel(double deltavelocity, double f) {
    // returns the amount of fuel needed for the reentry burn based on the reentry deltaV
    // calculates amount of fuel based on no drag, assuming a perfect burn. This does not account for engine lag, so the burn time is slightly lower but it should be okay. 
    double reentry_burn_fuel = 0.0;
    double deltaV = deltavelocity;

    double mass_flow_rate = FUEL_RATE_AT_MAX_THRUST * FUEL_DENSITY; // kg/s
    double exhaust_velocity = MAX_THRUST / mass_flow_rate; // m/s (thrust = mass_flow_rate * exhaust_velocity)
    // Tsiolkovsky rocket equation!
    reentry_burn_fuel = (get_lander_mass(f) * (1 - exp(-deltaV / exhaust_velocity))); // Fuel needed for the burn

    return reentry_burn_fuel;



}

/// Very optional function, lets see if it is good.
vector<vector3d> graphical_trajectory_computer(vector3d pos, vector3d vel, double f) {
    //This function takes the calculated deltaV, and uses it to find the predicted trajectory of the lander for graphics display while the pop up is open
    //Returns the trajectory points as a vector of vector3d objects.
    double t_max = simulation_time + get_time_period(position, velocity) * 1.5; // 1.5 is a safety factor, to ensure the lander can touch down within the next orbit.
    double dt = delta_t;
    double computer_time = simulation_time; // start time at the current simulation time - need to be careful when this updates!

    double computer_fuel = f; // bad prac?
    vector3d computer_position = pos;
    vector3d computer_velocity = vel;
    parachute_status_t computer_parachute_status = NOT_DEPLOYED; // current parachute status




    vector <vector3d> trajectory; // This will store the trajectory points - only need to store position points
    trajectory.push_back(computer_position); // Add initial position to trajectory

    computer_position = computer_position + computer_velocity * dt; // first step
    trajectory.push_back(computer_position); // Add first step position to trajectory

    // Verlet integration
    for (computer_time = computer_time + dt; computer_time <= t_max; computer_time = computer_time + dt) {

        // calculate new position and velocity
        vector3d computer_poslast = trajectory.end()[-2];

        // this could maaaybe be done better, it is already being calculated in the main loop, but not for in the future.
        vector3d acceleration = vector3d(0.0, 0.0, 0.0);
        acceleration += -gravity(computer_position) * computer_position.norm() / get_lander_mass(computer_fuel); // Gravitational acceleration
        acceleration += -drag(computer_velocity, computer_position, computer_parachute_status) * computer_velocity.norm() / get_lander_mass(computer_fuel); // Drag acceleration


        computer_position = 2 * computer_position - computer_poslast + dt * dt * acceleration; // Verlet integration step
        // no need for velocity, as we only need the position for the trajectory

        // append new position to trajectory
        trajectory.push_back(computer_position);
        if (computer_position.abs() < MARS_RADIUS) {
            break; // Stop if the lander has touched down
		}

    }
    return trajectory;
	cout << "Trajectory computed with " << trajectory.size() << " points." << endl;
}

double apoapsis_time; // Global variable to store the apoapsis time
double fuel_needed; // Global variable to store the reentry deltaV
double initialfuel; // Global variable to store the initial fuel in liters
vector3d apoapsis_velocity; // Global variable to store the apoapsis velocity

//Global variables that are reset by autopilot_initialize for the PID control
double previous_error = 0.0;
double integral = 0.0;

//TESTING VARIABLES
vector<double> SPlist, PVlist, throttlelist, timelist, altitudelist;

void autopilot_initialize(void) {
    cout << "\n=== AUTOPILOT INITIALIZATION ===" << endl;
    // cout << "Current autopilot mode: " << (autopilot_mode == MODE_ORBITAL_INJECTION ? "ORBITAL INJECTION" : "REENTRY") << endl;
    cout << "Current autopilot mode: " << autopilot_mode << endl;

    reentering = false;
    stabilized_reentry = false;
    orbital_injection_complete = false;

    reentry_data.data_calculated = false;
    reentry_data.zero_deltaV_warning = false;
    reentry_data.insufficient_fuel_warning = false;
    reentry_data.critical_fuel_warning = false;

    ApoapsiData apoapsis_data = apoapsis_computer(position, velocity, fuel);
    vector3d apoapsis_position = apoapsis_data.position;
    vector3d apoapsis_velocity = apoapsis_data.velocity;
    apoapsis_time = apoapsis_data.time;

    cout << "Apoapsis calculated at time: " << apoapsis_time << "s" << endl;
    cout << "Apoapsis altitude: " << (apoapsis_position.abs() - MARS_RADIUS) / 1000.0 << " km" << endl;

    reentry_data.apoapsis_radius = apoapsis_position.abs();
    reentry_data.apoapsis_velocity_magnitude = apoapsis_velocity.abs();
    reentry_data.apoapsis_time = apoapsis_time;

    if (autopilot_mode == MODE_ORBITAL_INJECTION) {
        // Calculate orbital injection parameters
        double injection_deltaV = get_injection_deltaV(apoapsis_position, apoapsis_velocity);
        double reentry_velocity = apoapsis_velocity.abs() + injection_deltaV;
        fuel_needed = get_reentry_burn_fuel(injection_deltaV, fuel);

		reentry_data.reentry_velocity = reentry_velocity;
        reentry_data.reentry_deltaV = injection_deltaV;

        // For orbital injection, show predicted orbit instead of reentry trajectory
        if (injection_deltaV > 0) {
            vector3d injection_velocity = apoapsis_velocity + (injection_deltaV * apoapsis_velocity.norm());
            reentry_data.predicted_trajectory = graphical_trajectory_computer(apoapsis_position, injection_velocity, fuel);
        }
        else {
            reentry_data.predicted_trajectory = graphical_trajectory_computer(apoapsis_position, apoapsis_velocity, fuel);
        }
    }
    else {
        // Calculate reentry parameters
        double reentry_deltaV = get_reentry_deltaV(apoapsis_position, apoapsis_velocity, fuel);
        double reentryvelocity = apoapsis_velocity.abs() - reentry_deltaV;
        fuel_needed = get_reentry_burn_fuel(reentry_deltaV, fuel);

        reentry_data.reentry_velocity = reentryvelocity;
        reentry_data.reentry_deltaV = reentry_deltaV;
        reentry_data.predicted_trajectory = graphical_trajectory_computer(apoapsis_position, apoapsis_velocity - (reentry_deltaV * apoapsis_velocity.norm()), fuel);

        if (reentry_deltaV == 0.0) {
            reentry_data.zero_deltaV_warning = true;
            reentering = true;
        }
    }

    initialfuel = fuel * FUEL_CAPACITY;
    reentry_data.initial_fuel = initialfuel;
    reentry_data.fuel_needed = fuel_needed;
    reentry_data.remaining_fuel = fuel * FUEL_CAPACITY - fuel_needed;

    if (fuel * FUEL_CAPACITY - fuel_needed < 0.0) {
        reentry_data.insufficient_fuel_warning = true;
        autopilot_enabled = false;
        reentry_data.data_calculated = true;
        autopilot_data_ready = true;
        return;
    }
    else if (fuel * FUEL_CAPACITY - fuel_needed < 35) {
        reentry_data.critical_fuel_warning = true;
    }

    integral = 0.0;
    previous_error = 0.0;

    reentry_data.data_calculated = true;
    autopilot_data_ready = true;
}

double get_setpoint(vector3d position) {
    // This function provides the setpoint for the terminal descent. The scheme is based off the Phoenix lander. https://ntrs.nasa.gov/api/citations/20080034648/downloads/20080034648.pdf
    // parachute deploy down to 1km, linear velocity to 8m/s at 50m, linear velocity to 0.5m/s at 0m.
	double altitude = position.abs() - MARS_RADIUS; // Calculate altitude from position
    if (altitude > 100.0) {
        double velocity_at_1000m = -45.0; //estimated parachute terminal velocity - could probably be better.
        return velocity_at_1000m + (-8.0 - velocity_at_1000m) * (-1000.0 + altitude) / (-1000.0 + 100.0);
    }

    else {
		double velocity_at_100m = -8.0; //velocity at 100m altitude
        return velocity_at_100m + (-0.8 - velocity_at_100m) * (-100 + altitude) / -100 ;
	}
}



double PIDcontrol(double SP, double Kp=0.06, double Ki = 0.006, double Kd = 0.04) {
    //function returns a throttle value (between 0 and 1) using PID control

    double PV = velocity * position.norm(); // process variable is the velocity in the radial direction. This is okay to get radial velocity, as tangential velocity is completely negligible at low alt

    double error = SP-PV;

    //P
    double Pout = Kp * error;
    
    //I
    integral += error * delta_t;
    double Iout = Ki * integral;

    //D
    double derivative = (error - previous_error) / delta_t;
    double Dout = Kd * derivative;
    previous_error = error;

    double output = Pout + Iout + Dout;

    //min max limiting
    if (output > 1.0) {
        output = 1.0;
    }
    else if (output < 0.0) {
        output = 0.0;
    }

    return output;
}

void autopilot_control(void) {
    if (autopilot_mode == MODE_ORBITAL_INJECTION && !orbital_injection_complete) {
        // ORBITAL INJECTION MODE

        // cout << "Orbital injection mode active. Apoapsis time: " << apoapsis_time << ", Current time: " << simulation_time << endl;

        // starts burn
        if (simulation_time > apoapsis_time && (fuel * FUEL_CAPACITY) > (initialfuel - fuel_needed)) {

            // Use orbital injection stabilization (points prograde)
            stabilized_reentry = false;
            stabilized_attitude = false;
            control_attitude = false;

            // This flag will be checked in the main simulation loop to call orbital_injection_stabilization()
            stabilized_injection = true;

            throttle = 1.0; // Maximum throttle for injection burn

            //cout << "Executing orbital injection burn at " << simulation_time << "s..." << endl;
            //cout << "Current velocity: " << velocity.abs() << " m/s" << endl;
        }

        // ends burn
        if (simulation_time > apoapsis_time && (fuel * FUEL_CAPACITY) <= (initialfuel - fuel_needed)) {
            throttle = 0.0;
            orbital_injection_complete = true;
            stabilized_injection = false; // deactivate injection stabilization

            cout << "\n" << "Orbital injection burn complete." << endl;

            // check if orbit is stable
            if (is_orbit_stable(position, velocity)) {
                cout << "\n" << "Stable orbit achieved!" << endl;
                cout << "Current altitude: " << (position.abs() - MARS_RADIUS) / 1000.0 << " km" << endl;
            }
            else {
                cout << "\n" << "Warning: Orbit may not be stable. Monitor trajectory." << endl;
            }
        }

        // After injection, maintain attitude stabilization (optional)
        if (orbital_injection_complete) {
            throttle = 0.0;
            stabilized_attitude = true;
        }

    }
    else if (autopilot_mode == MODE_REENTRY) {
        //// autopilot scheme: deploy parachute as early as possible, of course, then slow down. 
        // AUTOPILOT PROCEDURE - this for now is designed for a close/low orbit. elliptical orbits are possible. This does not work for a freefall drop - relies on drag to slow. 
        // 1) calculate a "reentry velocity" - an idea for this is the mininum velocity needed to touch the ground within the next orbit. MININUM IS IMPORTANT - guarantees large 
        // ground speed, which means high drag!
        // 
        // this needs a "computer" onboard which calculates the predicted trajectory. give some basic console output. Computer likely easiest method! seems difficult analytically.
        // "touch ground within next orbit" is good because it works for all kinds of orbits - return "NOT POSSIBLE WITHIN NEXT ORBIT" if more than 80% fuel is used.


        // 2) align lander against velocity vecotor, fire thruster for required deltaV

        // 3) engage stabilization program for safe reentry
        // 4) deploy parachute at appropriate altitude, if possible
        // 5) PID approach to land safely. 

        // "UI" considerations: hitting autopilot calculates deltaV, assuming firing thruster at periapsis (found by analysing orbit - function needed). after calculating, 
        // Print some results in console, "ENTER TO EXEC"

        // could add really cool ui element showing a projection of the trajectory, but this is not necessary for the basic functionality.




        //// Terminal descent, taken from phoenix lander data, https://ntrs.nasa.gov/api/citations/20080034648/downloads/20080034648.pdf
        // parachute deploy to 1km, linear velocity to 8m/s at 50m, linear velocity to 0.5m/s at 0m.
        // linear  velocity probably best, PID control - IDEA: use some AI integration to tune the PID controller for "aggressiveness" based on fuel reserves!!


        /* side note: the phoenix lander (2008) is a good model to base this project off of. 
        1) RCS only used for rate damping (instability at hypersonic speed) - a ballistic entry is used. This ensures AoA is already low, and with the low L/D of the capsule (0.06), and also the thin mars atmosphere,
        lift is largely negligible for this simulation. Accordingly, we can say the AoA = 0, and such our attitude stabilization system that keeps the capsule pointed against the velocity vector is a good one.
        2) 99% of KE is dissipated before parachute deployment. comparable to our system here
        3) supersonic parachute deployment. I estimate this is likely at the upper end of the parachute performance envelope, so this system is the same as ours too - deploying parachute asap. */

        // REENTRY BURN
        // Exec burn when at apoapsis
        // This fuel method is not very good. need to fix!
    

        // REENTRY MODE

        // starts burn
        if (simulation_time > apoapsis_time && (fuel * FUEL_CAPACITY) > (initialfuel - fuel_needed) && reentering == false) {
            throttle = 1.0;
            stabilized_reentry = true;
        }

        // ends burn
        if (simulation_time > apoapsis_time && (fuel * FUEL_CAPACITY) <= (initialfuel - fuel_needed) && reentering == false) {
            cout << "\n" << "Reentry burn complete. Lander is now reentering." << endl;
            throttle = 0.0;
            reentering = true;
        }

		// PARACHUTE DEPLOYMENT
        if (reentering && parachute_status == NOT_DEPLOYED) {
            if (safe_to_deploy_parachute()) {
                parachute_status = DEPLOYED;
                cout << "\n" << "Parachute deployed." << endl;
            }
        }
        // parachute jettison breaks the simulation for some reason
        /*else if (parachute_status == DEPLOYED && position.abs() - MARS_RADIUS < 10.0) {
            parachute_status = LOST;
            cout << "\n" << "Parachute jettisoned." << endl;
        }*/

        // TERMINAL DESCENT 
        // this line below just sets the altitude for terminal descent. after the lander goes below this altitude, setpoint is calculated once per cycle and throttle control is passed to pid
        if (position.abs() - MARS_RADIUS < 1000.0) {

            // limiting simulation speed, as the autopilot struggles to keep up otherwise.
            if (simulation_speed > 5) {
                simulation_speed = 5;
            }

            double targetvelocity = get_setpoint(position);
            throttle = PIDcontrol(targetvelocity);

            // TESTING PURPOSES: Save output and targetvelocity to lists, save lists to file for matplotlib plotting
            SPlist.push_back(targetvelocity);
            PVlist.push_back(velocity.x);
            throttlelist.push_back(throttle);
            timelist.push_back(simulation_time);
            altitudelist.push_back(position.abs() - MARS_RADIUS);

            // writes data to file.
            ofstream fout;
            fout.open("PIDcontroldata.txt");
            if (fout) {
                for (int i = 0; i < timelist.size(); i = i + 1) {
                    fout << timelist[i] << ' ' << SPlist[i] << ' ' << PVlist[i] << ' ' << throttlelist[i] << ' ' << altitudelist[i] << endl;
                }
            }
            else {
                cout << "Could not open trajectory file for writing" << endl;
            }

            // TESTING END
        }
    }
}