lander.cpp

// Mars lander simulator
// Version 1.11
// Mechanical simulation functions
// Gabor Csanyi and Andrew Gee, August 2019

// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation, to make use of it
// for non-commercial purposes, provided that (a) its original authorship
// is acknowledged and (b) no modified versions of the source code are
// published. Restriction (b) is designed to protect the integrity of the
// exercise for future generations of students. The authors would be happy
// to receive any suggested modifications by private correspondence to
// ahg@eng.cam.ac.uk and gc121@eng.cam.ac.uk.

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

double get_lander_mass (double f)
  // Returns the mass of the lander based on the current fuel level
{
  return UNLOADED_LANDER_MASS + f * FUEL_CAPACITY * FUEL_DENSITY; // (kg)
}


 double gravity(vector3d pos)
// Returns magnitude of gravitational force at position pos
{
  double r = pos.abs();
  if (r < MARS_RADIUS + LANDER_SIZE / 2.0) {
    // If the lander is below the surface, return zero gravity
    return 0.0;
  } else {
    // Gravitational force magnitude
    return GRAVITY * MARS_MASS * get_lander_mass(fuel)/ (r * r);
  }
}

 double drag(vector3d vel, vector3d pos, parachute_status_t parastat)
//Returns magnitude of drag force at velocity vel accounting for parachute status
{
	double lander_area = M_PI * LANDER_SIZE * LANDER_SIZE; // (m^2)
	double parachute_area = 5 * PARACHUTE_SIZE * PARACHUTE_SIZE; // (m^2)
    if (parastat == NOT_DEPLOYED) {
        //if parachute not deployed, drag only on lander
		return 0.5 * DRAG_COEF_LANDER * atmospheric_density(pos) * lander_area * vel.abs2();
    }
    if (parastat == DEPLOYED) {
		//if parachute deployed, drag on both lander and parachute
      
        return 0.5 * DRAG_COEF_LANDER * atmospheric_density(pos) * lander_area * vel.abs2() +
			0.5 * DRAG_COEF_CHUTE * atmospheric_density(pos) * parachute_area * vel.abs2();
        
    }
}

 // NUMERICAL SIMULATION CODE

// Add an extern declaration for first_step at the top of the file
extern bool first_step;

// Remove static from first_step in numerical_dynamics
void numerical_dynamics (void)
{
    static vector3d prev_position;

    // current acceleration
    vector3d acceleration = vector3d(0.0, 0.0, 0.0);
    acceleration += -gravity(position) * position.norm() / get_lander_mass(fuel); // Gravitational acceleration
    acceleration += -drag(velocity, position, parachute_status) * velocity.norm() / get_lander_mass(fuel); // Drag acceleration
    acceleration += thrust_wrt_world() / get_lander_mass(fuel); // Thrust acceleration

    if (first_step) {
        // Euler integration for the first step
        prev_position = position;
        position += velocity * delta_t + 0.5 * acceleration * delta_t * delta_t;
        velocity += acceleration * delta_t;
        first_step = false;
    } else {
        // Verlet integration for subsequent steps
        vector3d new_position = 2.0 * position - prev_position + acceleration * delta_t * delta_t;
        velocity = (new_position - prev_position) / (2.0 * delta_t); // Estimate velocity for output/drag
        prev_position = position;
        position = new_position;
    }

    // Autopilot and stabilization
    //if (autopilot_enabled && !autopilot_initialized) autopilot_initialize(); // Initialize autopilot only once
	if (stabilized_reentry) reentry_burn_stabilization(); // Stabilize attitude for reentry burn if autopilot is not initialized
    if (autopilot_enabled && autopilot_initialized) autopilot_control(); 
    if (stabilized_attitude) attitude_stabilization();
    if (stabilized_injection) orbital_injection_stabilization();
    if (control_attitude) attitude_control();
}

// Add a global definition for first_step in lander.cpp
bool first_step = true;

void initialize_simulation (void)
  // Lander pose initialization - selects one of 10 possible scenarios
{



  first_step = true; // Reset first_step to true for the simulation


  // The parameters to set are:
  // position - in Cartesian planetary coordinate system (m)
  // velocity - in Cartesian planetary coordinate system (m/s)
  // orientation - in lander coordinate system (xyz Euler angles, degrees)
  // delta_t - the simulation time step
  // boolean state variables - parachute_status, stabilized_attitude, autopilot_enabled
  // scenario_description - a descriptive string for the help screen

  scenario_description[0] = "circular orbit";
  scenario_description[1] = "descent from 10km";
  scenario_description[2] = "elliptical orbit, thrust changes orbital plane";
  scenario_description[3] = "polar launch at escape velocity (but drag prevents escape)";
  scenario_description[4] = "elliptical orbit that clips the atmosphere and decays";
  scenario_description[5] = "descent from 200km";
  scenario_description[6] = "";
  scenario_description[7] = "";
  scenario_description[8] = "";
  scenario_description[9] = "";

  switch (scenario) {

  case 0:
    // a circular equatorial orbit
    position = vector3d(1.2*MARS_RADIUS, 0.0, 0.0);
    velocity = vector3d(0.0, -3247.087385863725, 0.0);
    orientation = vector3d(0.0, 90.0, 0.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = false;
    autopilot_enabled = false;
    control_attitude = false;
	autopilot_initialized = false; // Initialize autopilot only once
    reentering = false;
    autopilot_data_ready = false;
	autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 1:
    // a descent from rest at 10km altitude
    position = vector3d(0.0, -(MARS_RADIUS + 10000.0), 0.0);
    velocity = vector3d(0.0, 0.0, 0.0);
    orientation = vector3d(0.0, 0.0, 90.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = true;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false; 
	autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 2:
    // an elliptical polar orbit
    position = vector3d(0.0, 0.0, 1.2*MARS_RADIUS);
    velocity = vector3d(3500.0, 0.0, 0.0);
    orientation = vector3d(0.0, 0.0, 90.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = false;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false;
    autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 3:
    // polar surface launch at escape velocity (but drag prevents escape)
    position = vector3d(0.0, 0.0, MARS_RADIUS + LANDER_SIZE/2.0);
    velocity = vector3d(0.0, 0.0, 5027.0);
    orientation = vector3d(0.0, 0.0, 0.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = false;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false;
    autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 4:
    // an elliptical orbit that clips the atmosphere each time round, losing energy
    position = vector3d(0.0, 0.0, MARS_RADIUS + 100000.0);
    velocity = vector3d(4000.0, 0.0, 0.0);
    orientation = vector3d(0.0, 90.0, 0.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = false;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false;
    autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 5:
    // a descent from rest at the edge of the exosphere
    position = vector3d(0.0, -(MARS_RADIUS + EXOSPHERE), 0.0);
    velocity = vector3d(0.0, 0.0, 0.0);
    orientation = vector3d(0.0, 0.0, 90.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = true;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false;
    autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 6:
    // an areostationary orbit
    position = vector3d(20429636, 0.0, 0.0);
    velocity = vector3d(0.0, 1448.1, 0.0);
    orientation = vector3d(0.0, 90.0, 0.0);
    delta_t = 0.1;
    parachute_status = NOT_DEPLOYED;
    stabilized_attitude = false;
    autopilot_enabled = false;
    control_attitude = false;
    autopilot_initialized = false;
    reentering = false;
    autopilot_data_ready = false;
    autopilot_mode = MODE_UNSELECTED;
    orbital_injection_complete = false;
    break;

  case 7:
      //terminal descent test
      position = vector3d(1500 + MARS_RADIUS, 0.0, 0.0);
      velocity = vector3d(-50, 0.0, 0.0);
      orientation = vector3d(0.0, 90.0, 0.0);
      delta_t = 0.1;
      parachute_status = NOT_DEPLOYED;
      stabilized_attitude = false;
      autopilot_enabled = true;
      control_attitude = false;
      autopilot_initialized = true;
      reentering = true;
      autopilot_data_ready = false;
      autopilot_mode = MODE_UNSELECTED;
      orbital_injection_complete = false;
    break;

  case 8:
    break;

  case 9:
    break;

  }
}