judsonupchurch/Airbreaks-Low-Power
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
main
Airbreaks-Low-Power/Programming/Arduino/Simulation/Environment_Simulator/Environment_Simulator.ino
SchrodingerError bc991043d9 First Commit
2023-12-19 18:30:36 -06:00

480 lines
24 KiB
C++
Raw Permalink Blame History

#include <rocket_simulator_datatypes.h>
#define ROCKET_MASS 630 // Initial mass of the rocket in grams, not including the mass of the propellant
#define CD_ROCKET 0.15 // Coefficient of drag of just the rocket without the petals or parachutes deployed
#define CD_PETALS 1.35 // Coefficient of drag of only the petals
#define CD_MAIN_PARACHUTE 0.8 // Coefficient of drag of only the main parachute
#define CD_DROGUE_PARACHUTE 0.7 // Coefficient of drag of only the drogue parachute. Assumed to be lower since it trails the main parachute
// All areas are the areas perpendicular to the flow of the air in meters squared except for the streamer. Its area is its total surface area
#define AREA_ROCKET 0.013685
// Area data for the petals as they are deployed at specific angles
// degree deployed
// area of single petal (mm^2)
float petal_area_data[2][10] {
{0,5,10,15,20,25,30,35,40,45},
{0,28.541,73.092,118.21,163.458,208.747,254.045,299.334,344.603,388.35}
};
/*
// {degree deployed, area (mm^2)}
float petal_area_data[10][2] {
{0, 0},
{5, 28.541},
{10, 073.092},
{15, 118.21},
{20, 163.458},
{25, 208.747},
{30, 254.045},
{35, 299.334},
{40, 344.603},
{45, 388.35},
};*/
// Area data for the main parachute
// cm of retraction (cm)
// area (m^2)
float main_parachute_area_data[2][2] {
{0, 10},
{0.3167, 0.25}
};
/*
// {cm of retraction (cm), area (m^2)
float main_parachute_area_data[2][2] {
{0, 0.3167},
{10, 0.25}
}*/
#define AREA_DROGUE_PARACHUTE 0.07944
// Data for the F52 Motor from Aerotech
#define TOTAL_IMPULSE 78 // Newton-seconds
// time(s)
// thrust(N)
// mass remaining(g)
float F52_motor_data[3][31] = {
{0,0.012,0.033,0.056,0.097,0.115,0.13,0.153,0.168,0.182,0.206,0.238,0.258,0.314,0.39,0.428,0.501,0.565,0.688,0.749,0.837,0.901,0.971,1.088,1.144,1.173,1.222,1.275,1.339,1.389,1.42},
{0,46.899,61.778,69.441,73.483,76.636,74.381,74.82,78.422,78.94,77.963,77.504,73.892,72.974,72.046,70.679,65.699,62.975,58.874,56.15,52.517,49.793,46.161,39.365,34.386,29.417,20.376,13.151,5.461,1.838,0},
{36.6,36.4588,35.8863,35.1292,33.336592,32.9814,32.4131,31.5523,30.9757,30.423,29.4783,28.2303,27.4707,25.4076,22.6428,21.2822,18.7848,16.719,12.9593,11.1992,8.80035,7.15779,5.47285,2.96266,1.92661,1.46246,0.850406,0.404654,0.105843,0.0142932,0}
};
/*
// {time (s), thrust (N), mass remaining (g)}
float F52_motor_data[31][3] = {
{0, 0, 36.6},
{0.012, 46.899, 36.4588},
{0.033, 61.778, 35.8863},
{0.056, 69.441, 35.1292},
{0.097, 73.483, 33.336592},
{0.115, 76.636, 32.9814},
{0.13, 74.381, 32.4131},
{0.153, 74.82, 31.5523},
{0.168, 78.422, 30.9757},
{0.182, 78.94, 30.423},
{0.206, 77.963, 29.4783},
{0.238, 77.504, 28.2303},
{0.258, 73.892, 27.4707},
{0.314, 72.974, 25.4076},
{0.39, 72.046, 22.6428},
{0.428, 70.679, 21.2822},
{0.501, 65.699, 18.7848},
{0.565, 62.975, 16.719},
{0.688, 58.874, 12.9593},
{0.749, 56.15, 11.1992},
{0.837, 52.517, 8.80035},
{0.901, 49.793, 7.15779},
{0.971, 46.161, 5.47285},
{1.088, 39.365, 2.96266},
{1.144, 34.386, 1.92661},
{1.173, 29.417, 1.46246},
{1.222, 20.376, 0.850406},
{1.275, 13.151, 0.404654},
{1.339, 5.461, 0.105843},
{1.389, 1.838, 0.0142932},
{1.42, 0, 0}
};*/
//Environmental and simulation factors
#define AIR_DENSITY 1.225 // kg/m^3
#define ACCELEROMETER_ERROR 0 // Percent of error randomly applied to the calculated then sent accelerometer readings
#define ALTIMETER_ERROR 0 // Percent of error randomly applied to the calculated then sent altimeter readings
#define FREQUENCY 10 // How many times per second the simulator should send information to the flight computer
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(9600); // Init serial with the PC
//Serial.println("Beginning Flight Simulation");
//Serial.println("Air Density Accel Error Alt Error");
//Serial.print(AIR_DENSITY); Serial.print(" "); Serial.print(ACCELEROMETER_ERROR); Serial.print(" "); Serial.println(ALTIMETER_ERROR);
Serial1.begin(9600); // Init serial with the flight computer
randomSeed(analogRead(0));
delay(500);
while (!flight_computer_ready()); // Make sure that the flight computer has been initialized before sending data
delay(500);
Serial.println();
}
// Global variable to keep track of the actual data for the rocket so the simulator can provide real data
actual_rocket_data_struct actual_rocket_data {
0.0, // Petal angle (deg)
0.0, // Main parachute retract distance (cm)
0, // Stage of the rocket.
//0 = on launch rod, 1 = after motor ignition
//2 = after self propelled flight, 3 = after motor burnout
//4 = after apogee, 5 = terminal velocity
//6 = after ground hit
ROCKET_MASS + F52_motor_data[0][2], // Mass (g)
0.0, // Altitude (m)
{0.0, 0.0, 0.0}, // xyz_float of velocity (m/s)
{0.0, 0.0, 0.0}, // xyz_float of acceleration (m/s/s)
0.0, // Flight time (s)
0.0, // Engine thrust (N)
0.0, // Rocket drag (N)
0.0, // Petal drag (N)
false, // Parachutes deployed
0.0, // Main parachute drag (N)
0.0 // Drogue parachute drag (N)
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
calculate_data();
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool send_data(data_packet_struct data) {
static bool first_time = true;
if (first_time) {
// For testing
/*
Serial.print("Time ");
Serial.print("Alt ");
Serial.print("Accel ");
Serial.println();
*/
first_time = false;
}
static double next_flight_report = 0;
if (data.flight_time > next_flight_report) {
// For testing
//Serial.print(data.flight_time); Serial.print(", ");
//Serial.print(data.alt); Serial.print(", ");
//Serial.print(data.accel.z);
// For actually sending to the flight controller
Serial1.print(data.flight_time); Serial1.print("T");
Serial1.print(data.alt); Serial1.print("A");
Serial1.print(data.accel.z); Serial1.print("Z");
//Serial.println();
Serial1.println("!"); // Signifiy that we have finished sending the data for this one report
next_flight_report = data.flight_time + double(1.0/FREQUENCY);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void plot_data(actual_rocket_data_struct data) {
static bool first_time = true;
if (first_time) {
Serial.print("Time ");
Serial.print("Stage ");
Serial.print("Alt ");
Serial.print("Vel ");
Serial.print("Accel ");
//Serial.print("Thrust ");
Serial.print("RocketDrag ");
Serial.print("PetalDrag ");
//Serial.print("MainDrag ");
//Serial.print("DrogueDrag ");
Serial.println();
first_time = false;
}
static double next_flight_report = 0;
float fc_pred_alt = 0.0;
if (Serial1.available()) { // Try to receive the flight computer outputting its data
String msg = Serial1.readStringUntil('!');
fc_pred_alt = msg.substring(0, msg.indexOf('P')).toFloat();
}
if (data.flight_time > next_flight_report) {
Serial.print(data.flight_time); Serial.print(", ");
Serial.print(data.stage); Serial.print(", ");
Serial.print(data.alt); Serial.print(", ");
Serial.print(data.velocity.z); Serial.print(", ");
Serial.print(data.accel.z); Serial.print(", ");
//Serial.print(data.engine_thrust); Serial.print(", ");
Serial.print(data.rocket_drag); Serial.print(", ");
Serial.print(data.petal_drag); Serial.print(", ");
//Serial.print(data.main_parachute_drag); Serial.print(", ");
//Serial.print(data.drogue_parachute_drag); Serial.print(",");
Serial.print(fc_pred_alt);
Serial.println();
next_flight_report = data.flight_time + double(1.0/FREQUENCY);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
data_packet_struct calculate_data() {
//Continuously integrate the actual rocket data in order to simulate a launch
//integration_delay_micros is the number of microseconds since the last time the actual_rocket_data was updated/integrated
static uint32_t last_micros = micros();
static uint32_t current_micros;
current_micros = micros();
double integration_delay_seconds = (current_micros-last_micros)*0.000001;
last_micros = current_micros;
// Integrate velocity for a new altitude
actual_rocket_data.alt += actual_rocket_data.velocity.z * integration_delay_seconds;
// Integrate the acceleration for a new velocity
actual_rocket_data.velocity.z += actual_rocket_data.accel.z * integration_delay_seconds;
// Calculate the new acceleration of the rocket
float net_forces = 0;
switch (actual_rocket_data.stage) {
case 0: // Still on the launch rod, no motor ignition
net_forces = 0; // Redundant
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Because we are the simulator, we can control when liftoff occurs. We will wait to receive an "I" from serial
while (!has_ignition_occurred()); // Wait for the person to send an "I" in the Serial monitor to signify ignition
actual_rocket_data.stage = 1;
last_micros = micros();
current_micros = last_micros;
return;
break;
case 1: // Still on the launch rod with motor ignition
net_forces = 0; // Redundant
// Check to see if we are now under self-propelled flight, aka, engine thrust > weight, aka liftoff
// Gravity
net_forces += -0.00981*(actual_rocket_data.mass);
// Rocket thrust
actual_rocket_data.engine_thrust = interpolate_data(F52_motor_data[0], F52_motor_data[1], 31, actual_rocket_data.flight_time);
net_forces += actual_rocket_data.engine_thrust;
// No need to check the drag because a non-zero velocity is required before achieving drag
// Calculate acceleration
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Iterate the flight time
actual_rocket_data.flight_time += integration_delay_seconds;
// Checking when to move to next stage
if (actual_rocket_data.accel.z > 0.001) { // We have liftoff
actual_rocket_data.stage = 2;
}
break;
case 2: // No longer supported by the launch rod, self-propelled flight, after liftoff
// Gravity
net_forces += -0.00981*(actual_rocket_data.mass);
// Rocket thrust
actual_rocket_data.engine_thrust = interpolate_data(F52_motor_data[0], F52_motor_data[1], 31, actual_rocket_data.flight_time);
net_forces += actual_rocket_data.engine_thrust;
// Rocket body drag
actual_rocket_data.rocket_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.rocket_drag;
// Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
actual_rocket_data.petal_drag = -1*sign(actual_rocket_data.velocity.z)*0.000003*calculate_drag(CD_PETALS, interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.petal_angle), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.petal_drag;
// Calculate acceleration
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Iterate the flight time
actual_rocket_data.flight_time += integration_delay_seconds;
// Checking when to move to next stage
if (actual_rocket_data.flight_time >= F52_motor_data[0][30]) { // We have motor burnout
actual_rocket_data.stage = 3;
}
break;
case 3: // After motor burnout has occurred
// Gravity
net_forces += -0.00981*(actual_rocket_data.mass);
// Rocket body drag
actual_rocket_data.rocket_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.rocket_drag;
// Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
actual_rocket_data.petal_drag = -1*sign(actual_rocket_data.velocity.z)*0.000003*calculate_drag(CD_PETALS, interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.petal_angle), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.petal_drag;
// Calculate acceleration
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Iterate the flight time
actual_rocket_data.flight_time += integration_delay_seconds;
// Checking when to move to next stage
if (actual_rocket_data.accel.z <= -5) {
if (actual_rocket_data.velocity.z >= -5 && actual_rocket_data.velocity.z <= 5) {
actual_rocket_data.stage = 4;
}
}
break;
case 4: // After apogee, assume parachute release at apogee
// Gravity
net_forces += -0.00981*(actual_rocket_data.mass);
// Rocket body drag
actual_rocket_data.rocket_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.rocket_drag;
// Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
actual_rocket_data.petal_drag = -1*sign(actual_rocket_data.velocity.z)*0.000003*calculate_drag(CD_PETALS, interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.petal_angle), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.petal_drag;
// Drogue parachute drag which should be relatively constant besides changes in velocity because it stays the same size
actual_rocket_data.drogue_parachute_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_DROGUE_PARACHUTE, AREA_DROGUE_PARACHUTE, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.drogue_parachute_drag;
// Main parachute drag which changes due to its changing size
actual_rocket_data.main_parachute_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_MAIN_PARACHUTE, interpolate_data(main_parachute_area_data[0], main_parachute_area_data[1], 2, actual_rocket_data.main_parachute_retract), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.main_parachute_drag;
// Calculate acceleration
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Iterate the flight time
actual_rocket_data.flight_time += integration_delay_seconds;
// Checking when to move to next stage
if (actual_rocket_data.accel.z >= -.01 && actual_rocket_data.accel.z <= 0.01) {
actual_rocket_data.stage = 5;
}
break;
case 5: // After reaching normal terminal velocity from the parachutes
// Gravity
net_forces += -0.00981*(actual_rocket_data.mass);
// Rocket body drag
actual_rocket_data.rocket_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_ROCKET, AREA_ROCKET, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.rocket_drag;
// Petal drag. Multiply by 0.000003 because the area is in mm^2 and there are 3 petals total
actual_rocket_data.petal_drag = -1*sign(actual_rocket_data.velocity.z)*0.000003*calculate_drag(CD_PETALS, interpolate_data(petal_area_data[0], petal_area_data[1], 10, actual_rocket_data.petal_angle), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.petal_drag;
// Drogue parachute drag which should be relatively constant besides changes in velocity because it stays the same size
actual_rocket_data.drogue_parachute_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_DROGUE_PARACHUTE, AREA_DROGUE_PARACHUTE, actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.drogue_parachute_drag;
// Main parachute drag which changes due to its changing size
actual_rocket_data.main_parachute_drag = -1*sign(actual_rocket_data.velocity.z)*calculate_drag(CD_MAIN_PARACHUTE, interpolate_data(main_parachute_area_data[0], main_parachute_area_data[1], 2, actual_rocket_data.main_parachute_retract), actual_rocket_data.velocity.z);
net_forces += actual_rocket_data.main_parachute_drag;
// Calculate acceleration
actual_rocket_data.accel.z = (net_forces) / ((actual_rocket_data.mass)*0.001);
// Iterate the flight time
actual_rocket_data.flight_time += integration_delay_seconds;
// Checking when to move to next stage
if (actual_rocket_data.alt <= 0) {
actual_rocket_data.stage = 6;
}
break;
case 6: // After the parachute has reached the ground
Serial.println("Hit Ground");
while(1);
break;
}
data_packet_struct return_data {
0.0,
{0.0,0.0,0.0},
0.0
};
return_data.alt = (actual_rocket_data.alt)*(1.0 + random(-1*ALTIMETER_ERROR*100, ALTIMETER_ERROR*100+1)*0.0001);
return_data.accel.x = (actual_rocket_data.accel.x)*(1.0 + random(-1*ACCELEROMETER_ERROR*100, ACCELEROMETER_ERROR*100+1)*0.0001);
return_data.accel.y = (actual_rocket_data.accel.y)*(1.0 + random(-1*ACCELEROMETER_ERROR*100, ACCELEROMETER_ERROR*100+1)*0.0001);
return_data.accel.z = (actual_rocket_data.accel.z)*(1.0 + random(-1*ACCELEROMETER_ERROR*100, ACCELEROMETER_ERROR*100+1)*0.0001);
return_data.flight_time = actual_rocket_data.flight_time;
//plot_data(actual_rocket_data);
send_data(return_data);
return return_data;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//A function used to interpolate data from an array of x values and their corresponding y values
float interpolate_data(float *x_data_ptr, float *y_data_ptr, int num_x_vals, float actual_x_val) {
if (actual_x_val > *(x_data_ptr + num_x_vals-1)) {
return 0;
}
int lower_bound_index;
for (int i = 0; i < num_x_vals; i++) {
if (actual_x_val == *(x_data_ptr + i)) { // If the x value we're looking at lands exactly at a data point, just use it
return *(y_data_ptr + i); // Return the y value associated with the x value we are looking at
} else if (actual_x_val > *(x_data_ptr + i)) { // The x value we're looking for is greater than the value we're comparing it to, then we just passed the suitable range
lower_bound_index = i; // Save the index of the last x value where we are greater than
} else { // If the x value we're looking for is less than the value we're comparing it to, stop
break;
}
}
// Now that we have the lower bound index, we know which two x values the data point will fall between,
// so we create a secant line between the two bounds and then grab the estimated y value
// y = mx + b -> y = (y2-y1)/(x2-x1)*(x-x1) + y1
// where (x1,y1) is the lower bound data point and (x2,y2) is the upper bound data point
return ((*(y_data_ptr+lower_bound_index+1) - *(y_data_ptr+lower_bound_index))/(*(x_data_ptr+lower_bound_index+1) - *(x_data_ptr+lower_bound_index)))*(actual_x_val - *(x_data_ptr+lower_bound_index)) + *(y_data_ptr+lower_bound_index);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Simple function to return the drag of an object
float calculate_drag(float cd, float area, float velocity) {
return 0.5 * AIR_DENSITY * velocity * velocity * cd * area;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int sign(float x) {
if (x == 0) {
return 0;
} else if (x < 0) {
return -1;
} else {
return 1;
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool has_ignition_occurred() {
if (Serial1.available()) {
String msg = Serial1.readStringUntil('!');
if (msg == "I") {
return true;
Serial.println("Ignition");
Serial1.println();
} else {
return false;
}
}
return false;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool flight_computer_ready() {
Serial1.print("S!"); // Tell the flight computer that we are starting
if (Serial1.available()) {
String msg = Serial1.readStringUntil('!');
if (msg == "S") {
return true;
Serial.println("Flight Computer Started");
Serial1.println();
} else {
return false;
}
}
return false;
}
Reference in New Issue View Git Blame Copy Permalink