53 lines
2.2 KiB
Plaintext
53 lines
2.2 KiB
Plaintext
void integrate_data(data_packet_struct received_data) {
|
|
static double last_flight_time = 0.0;
|
|
static double current_flight_time;
|
|
current_flight_time = received_data.flight_time;
|
|
double integration_delay_seconds = (current_flight_time-last_flight_time);
|
|
last_flight_time = current_flight_time;
|
|
|
|
// Altimeter velocity
|
|
fc_data.altimeter_velocity = (received_data.alt - fc_data.altimeter_altitude) / integration_delay_seconds;
|
|
// Altimeter altitude
|
|
fc_data.altimeter_altitude = received_data.alt;
|
|
|
|
// Integrate velocity for a new altitude
|
|
fc_data.integration_altitude += fc_data.velocity.z * integration_delay_seconds;
|
|
// Integrate the acceleration for a new velocity
|
|
fc_data.velocity.z += fc_data.accel.z * integration_delay_seconds;
|
|
// Save the acceleration
|
|
fc_data.accel.z = received_data.accel.z;
|
|
|
|
// Subtract gravity from acceleration to get total drag
|
|
fc_data.total_drag = (abs(fc_data.accel.z)-9.81)*0.63;
|
|
|
|
fc_data.flight_time = received_data.flight_time;
|
|
|
|
|
|
|
|
// Setting petal angle
|
|
if (fc_data.altimeter_altitude > 100) {
|
|
float buff = fc_data.prediction_altitude;
|
|
float coasting_distance = abs(((0.63*fc_data.velocity.z*fc_data.velocity.z)/(2.0*fc_data.total_drag))*log(1+((fc_data.total_drag)/(0.63*9.81))));
|
|
fc_data.prediction_altitude = fc_data.altimeter_altitude + coasting_distance;
|
|
if (fc_data.prediction_altitude >= 1000) {
|
|
fc_data.prediction_altitude = 0;
|
|
fc_data.prediction_altitude_roc = 0;
|
|
} else {
|
|
fc_data.prediction_altitude_roc = (fc_data.prediction_altitude - buff)/integration_delay_seconds;
|
|
}
|
|
|
|
if (fc_data.prediction_altitude > TARGET_ALTITUDE) { // If we are going to go too high
|
|
if (fc_data.petal_angle < 45) {
|
|
fc_data.petal_angle = fc_data.petal_angle + 0.3;
|
|
}
|
|
} else if (fc_data.prediction_altitude < TARGET_ALTITUDE) {
|
|
if (fc_data.petal_angle > 0) {
|
|
fc_data.petal_angle = fc_data.petal_angle - 0.3;
|
|
}
|
|
}
|
|
} else {
|
|
fc_data.prediction_altitude = 0;
|
|
fc_data.prediction_altitude_roc = 0;
|
|
}
|
|
|
|
} |