#include #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; }