diff options
Diffstat (limited to 'src/active_drag_system.cpp')
| -rw-r--r-- | src/active_drag_system.cpp | 1131 |
1 files changed, 770 insertions, 361 deletions
diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp index 6f5d57e..ce898bb 100644 --- a/src/active_drag_system.cpp +++ b/src/active_drag_system.cpp @@ -1,436 +1,845 @@ -#include "cyw43_configport.h" -#include "pico/multicore.h" -#include "pico/platform.h" -#include "pico/sem.h" -#include "boards/pico_w.h" +#include <pico.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <inttypes.h> + #include "hardware/gpio.h" #include "hardware/i2c.h" #include "hardware/adc.h" +#include <hardware/timer.h> +#include "pico/rand.h" #include "pico/stdlib.h" +#include "pico/stdio.h" +#include "pico/multicore.h" #include "pico/time.h" -#include "pico/types.h" -#include "pico/cyw43_arch.h" -#include <inttypes.h> -#include <math.h> +#include <pico/error.h> +#include <pico/types.h> +#include "math.h" + +#include "FreeRTOS.h" +#include "FreeRTOSConfig.h" +#include "portmacro.h" +#include "projdefs.h" +#include "serial.hpp" +#include "task.h" +#include "semphr.h" + +extern "C" { +#include <fix16.h> +#include <fixmatrix.h> +#include <fixkalman.h> +} +#include "adxl375.hpp" +#include "ms5607.hpp" +#include "iim42653.hpp" +#include "mmc5983ma.hpp" #include "pwm.hpp" -#include "imu.hpp" -#include "altimeter.hpp" -#include "kalman_filter.hpp" -#include "spi_flash.h" - -#define MPL3115A2_ADDR 0x60 - -#define BNO055_ADDR 0x28 -#define BNO055_ID 0xA0 - -#define MAX_SCL 400000 -#define DATA_RATE_HZ 100 -#define LOOP_PERIOD (1.0f / DATA_RATE_HZ) -#define INT1_PIN 6 // INT1 PIN on MPL3115A2 connected to GPIO PIN 9 (GP6) -#define MOSFET_PIN 26 // MOSFET PIN connected to GPIO PIN 31 (GP26) - -#define LOG_RATE_HZ 8 -#define HEART_RATE_HZ 5 - -#define MOTOR_BURN_TIME 3900 // Burn time in milliseconds for M2500T - -#define PAD_SECONDS 8 -#define PAD_BUFFER_SIZE (PACKET_SIZE * LOG_RATE_HZ * PAD_SECONDS) - -typedef enum { - PAD = 0, - BOOST, - COAST, - APOGEE, - RECOVERY, - END -} state_t; +#include "pico_logger.h" +#include "log_format.hpp" +#include "serial.hpp" +#include "heartbeat.hpp" + +/****************************** FREERTOS **************************************/ +#define SENSOR_EVENT_HANDLER_PRIORITY ( tskIDLE_PRIORITY + 7 ) +#define SENSOR_SAMPLE_PRIORITY ( tskIDLE_PRIORITY + 6 ) +#define KALMAN_TASK_PRIORITY ( tskIDLE_PRIORITY + 6 ) +#define ROCKET_TASK_PRIORITY ( tskIDLE_PRIORITY + 4 ) +#define ROCKET_EVENT_HANDLER_PRIORITY ( tskIDLE_PRIORITY + 3 ) +#define HEARTBEAT_TASK_PRIORITY ( tskIDLE_PRIORITY + 2 ) +#define LOGGING_PRIORITY ( tskIDLE_PRIORITY + 2 ) +#define SERIAL_TASK_PRIORITY ( tskIDLE_PRIORITY + 1 ) + +void vApplicationTickHook(void) { /* optional */ } +void vApplicationMallocFailedHook(void) { /* optional */ } +void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { for( ;; ); } + +static void logging_task(void* pvParameters); + +#define ROCKET_TASK_RATE_HZ 50 +volatile TaskHandle_t rocket_task_handle = NULL; +static void rocket_task(void* pvParameters); + +#define KALMAN_TASK_RATE_HZ 50 +volatile TaskHandle_t kalman_task_handle = NULL; +static void kalman_task(void* pvParameters); + +int64_t launch_event_callback(alarm_id_t id, void* user_data); +int64_t coast_event_callback(alarm_id_t id, void* user_data); +int64_t end_event_callback(alarm_id_t id, void* user_data); + +volatile TaskHandle_t launch_event_handler_task_handle = NULL; +volatile TaskHandle_t coast_event_handler_task_handle = NULL; +volatile TaskHandle_t end_event_handler_task_handle = NULL; +static void launch_event_handler_task(void* pvParameters); +static void coast_event_handler_task(void* pvParameters); +static void end_event_handler_task(void* pvParameters); + +/****************************** FREERTOS **************************************/ + +/****************************** SERIAL CONSOLE ********************************/ +static void read_cmd_func(); +static void write_cmd_func(); +static void erase_cmd_func(); +static void show_cmd_func(); +static void deploy_cmd_func(); +static void kalman_cmd_func(); + +const char* executeable_name = "active-drag-system.uf2"; +const size_t num_user_cmds = 6; +const command_t user_commands[] = { {.name = "read", + .len = 4, + .function = &read_cmd_func}, + {.name = "write", + .len = 5, + .function = &write_cmd_func}, + {.name = "erase", + .len = 5, + .function = &erase_cmd_func}, + {.name = "show", + .len = 4, + .function = &show_cmd_func}, + {.name = "deploy", + .len = 6, + .function = &deploy_cmd_func}, + {.name = "kalman", + .len = 6, + .function = &kalman_cmd_func} }; +/****************************** SERIAL CONSOLE ********************************/ + +/****************************** LOGGING ***************************************/ +volatile bool use_circular_buffer = true; +volatile TaskHandle_t logging_handle = NULL; +volatile log_entry_t log_entry; + +Logger logger(PACKET_SIZE, LOG_BASE_ADDR, print_log_entry); + +static void populate_log_entry(log_entry_t* log_entry); +/****************************** LOGGING ***************************************/ + +volatile bool serial_data_output = false; + +MS5607 alt(i2c_default); +ADXL375 adxl375(i2c_default); +IIM42653 iim42653(i2c_default); +MMC5983MA mmc5983ma(i2c_default); PWM pwm; -kalman_filter *kf; -VectorXf control(1); -VectorXf measurement(1); -VectorXf res(2); - -void pad_callback(uint gpio, uint32_t event_mask); -int64_t boost_callback(alarm_id_t id, void* user_data); -int64_t apogee_callback(alarm_id_t id, void* user_data); -int64_t coast_callback(alarm_id_t id, void* user_data); -void recovery_callback(uint gpio, uint32_t event_mask); -float get_deploy_percent(float velocity, float altitude); - -bool timer_callback(repeating_timer_t *rt); - -void populate_entry(); -bool logging_buffer_callback(repeating_timer_t *rt); -bool logging_flash_callback(repeating_timer_t *rt); -bool heartbeat_callback(repeating_timer_t *rt); -void logging_core(); - -semaphore_t sem; - -volatile float altitude = 0.0f; -volatile float velocity = 0.0f; -volatile state_t state = PAD; -volatile float threshold_altitude = 30.0f; -volatile float threshold_velocity = 30.0f; -volatile uint8_t deployment_percent = 0; -volatile uint8_t led_counter; -volatile uint32_t pad_buffer_offset = 0; -Eigen::Vector3f linear_acceleration; -Eigen::Vector4f quaternion; -Eigen::Vector3f euler_angles; - -volatile calibration_status_t calib_status; - -repeating_timer_t data_timer; -repeating_timer_t log_timer; -repeating_timer_t heartbeat_timer; - -float ground_altitude = 0.0f; +#define MOTOR_BURN_TIME 6200 +volatile state_t rocket_state = PAD; +volatile uint8_t deployment_percent = 0; -altimeter altimeter(i2c_default, MPL3115A2_ADDR); -imu imu(i2c_default, BNO055_ADDR, BNO055_ID, NDOF); +volatile int32_t ground_altitude = 0; +volatile int32_t threshold_altitude = 30; + +/****************************** KALMAN ****************************************/ +#define KALMAN_NAME verticality +#define KALMAN_NUM_STATES 2 +#define KALMAN_NUM_INPUTS 1 +kalman16_t kf; + +#define KALMAN_MEASUREMENT_NAME altitude +#define KALMAN_NUM_MEASUREMENTS 1 +kalman16_observation_t kfm; + +#define matrix_set(matrix, row, column, value) \ + matrix->data[row][column] = value + +#define matrix_set_symmetric(matrix, row, column, value) \ + matrix->data[row][column] = value; \ + matrix->data[column][row] = value + +#ifndef FIXMATRIX_MAX_SIZE +#error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements. +#endif + +#if (FIXMATRIX_MAX_SIZE < KALMAN_NUM_STATES) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_INPUTS) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_MEASUREMENTS) +#error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements. +#endif + +static void kalman_verticality_init() { + /************************************************************************/ + /* initialize the filter structures */ + /************************************************************************/ + kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS); + kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS); + + /************************************************************************/ + /* set initial state */ + /************************************************************************/ + mf16 *x = kalman_get_state_vector(&kf); + x->data[0][0] = 0; // s_i + x->data[1][0] = 0; // v_i + + /************************************************************************/ + /* set state transition */ + /************************************************************************/ + mf16 *A = kalman_get_state_transition(&kf); + + // set time constant +#if (DEBUG == 1) + const fix16_t T = fix16_from_float(0.02f); +#else + const fix16_t T = fix16_from_float(0.02f); +#endif + const fix16_t Tsquare = fix16_sq(T); + + // helper + const fix16_t fix16_half = fix16_from_float(0.5); + + // transition of x to s + matrix_set(A, 0, 0, fix16_one); // 1 + matrix_set(A, 0, 1, T); // T + + // transition of x to v + matrix_set(A, 1, 0, 0); // 0 + matrix_set(A, 1, 1, fix16_one); // 1 + + + /************************************************************************/ + /* set covariance */ + /************************************************************************/ + mf16 *P = kalman_get_system_covariance(&kf); + +// matrix_set_symmetric(P, 0, 0, fix16_half); // var(s) +// matrix_set_symmetric(P, 0, 1, 0); // cov(s,v) +// matrix_set_symmetric(P, 0, 2, 0); // cov(s,g) +// +// matrix_set_symmetric(P, 1, 1, fix16_one); // var(v) +// matrix_set_symmetric(P, 1, 2, 0); // cov(v,g) +// +// matrix_set_symmetric(P, 2, 2, fix16_one); // var(g) + + /************************************************************************/ + /* set input covariance */ + /************************************************************************/ + mf16 *Q = kalman_get_input_covariance(&kf); +// mf16_fill_diagonal(Q, fix16_one); + mf16_mul_bt(Q, A, A); + mf16_mul_s(Q, Q, fix16_from_int(10*10)); + + /************************************************************************/ + /* set control input transformation */ + /************************************************************************/ + mf16 *B = kalman_get_input_transition(&kf); + matrix_set(B, 0, 0, fix16_mul(fix16_half, Tsquare)); // u = 0*s + matrix_set(B, 1, 0, T); // + 0*v + + /************************************************************************/ + /* set measurement transformation */ + /************************************************************************/ + mf16 *H = kalman_get_observation_transformation(&kfm); + matrix_set(H, 0, 0, fix16_one); // z = 1*s + matrix_set(H, 0, 1, 0); // + 0*v + + /************************************************************************/ + /* set process noise */ + /************************************************************************/ + mf16 *R = kalman_get_observation_process_noise(&kfm); + + matrix_set(R, 0, 0, fix16_from_int(36)); // var(s) +} -uint8_t *altimeter_buffer; -uint8_t *acceleration_buffer; -uint8_t *quaternion_buffer; +volatile bool derate_baro_sensor = false; +const fix16_t baro_velocity_derate = F16(160.f); + +volatile fix16_t altitude_filt = 0; +volatile fix16_t velocity_filt = 0; + +volatile fix16_t drag_force = 0; +volatile fix16_t apogee_prediction = 0; +volatile fix16_t desired_drag_force = 0; +volatile fix16_t desired_deployment = 0; + +void kalman_update(fix16_t altitude, fix16_t vertical_acceleration) { + static mf16* control = kalman_get_input_vector(&kf); + static mf16* measurement = kalman_get_observation_vector(&kfm); + static mf16* Q = kalman_get_input_covariance(&kf); + static mf16 *R = kalman_get_observation_process_noise(&kfm); + static mf16* state_vector = kalman_get_state_vector(&kf); + + if (state_vector->data[1][0] >= baro_velocity_derate && !derate_baro_sensor) { + mf16_div_s(Q, Q, fix16_from_int(3)); + mf16_mul_s(R, R, fix16_from_int(4)); + derate_baro_sensor = true; + } else if (state_vector->data[1][0] < baro_velocity_derate && derate_baro_sensor) { + mf16_mul_s(Q, Q, fix16_from_int(3)); + mf16_div_s(R, R, fix16_from_int(4)); + derate_baro_sensor = false; + } + matrix_set(control, 0, 0, vertical_acceleration); + kalman_predict(&kf); + matrix_set(measurement, 0, 0, altitude); + kalman_correct(&kf, &kfm); +} -uint8_t entry_buffer[PACKET_SIZE]; +fix16_t calculate_drag_force(fix16_t deployment_percentage, fix16_t vertical_velocity); +fix16_t predict_apogee(fix16_t altitude, fix16_t vertical_velocity, fix16_t drag_force); +fix16_t calculate_deployment_percentage(fix16_t drag_force, fix16_t vertical_velocity); +fix16_t calculate_desired_drag_force(fix16_t altitude, fix16_t vertical_velocity); -uint8_t *pad_buffer; +/****************************** KALMAN ****************************************/ int main() { + stdio_init_all(); + adc_init(); adc_set_temp_sensor_enabled(true); - cyw43_arch_init(); + heartbeat_initialize(PICO_DEFAULT_LED_PIN); - i2c_init(i2c_default, MAX_SCL); + i2c_init(i2c_default, 400000); gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C); gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C); - gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN); - gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN); - - // Enable SPI 0 at 60 MHz and connect to GPIOs - spi_init(spi_default, 1000 * 1000 * 60); - gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI); - gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI); - gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI); - - // Chip select is active-low, so we'll initialise it to a driven-high state - gpio_init(PICO_DEFAULT_SPI_CSN_PIN); - gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT); - gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1); - - gpio_init(INT1_PIN); - gpio_pull_up(INT1_PIN); - - alarm_pool_init_default(); - altimeter.initialize(30.0f, INT1_PIN, &pad_callback); + gpio_init(MICRO_DEFAULT_SERVO_ENABLE); + gpio_set_dir(MICRO_DEFAULT_SERVO_ENABLE, GPIO_OUT); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0); + + sleep_ms(2500); + + info_cmd_func(); + stdio_flush(); - imu.initialize(); - imu.linear_acceleration(linear_acceleration); - imu.quaternion(quaternion); - imu.quaternion_euler(euler_angles, quaternion); + logger.initialize(true); + logger.initialize_circular_buffer(PAD_BUFFER_SIZE); + alt.initialize(); + sleep_ms(500); + adxl375.initialize(); + sleep_ms(500); + iim42653.initialize(); + sleep_ms(500); + mmc5983ma.initialize(); + sleep_ms(500); pwm.init(); - // Initialize MOSFET - gpio_init(MOSFET_PIN); - gpio_set_dir(MOSFET_PIN, GPIO_OUT); + kalman_verticality_init(); - cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1); + xTaskCreate(heartbeat_task, "heartbeat", 256, NULL, HEARTBEAT_TASK_PRIORITY, NULL); + xTaskCreate(serial_task, "serial", 8192, NULL, SERIAL_TASK_PRIORITY, NULL); - pad_buffer = (uint8_t*)malloc(PAD_BUFFER_SIZE); + xTaskCreate(MS5607::update_ms5607_task, "update_ms5607", 256, &alt, SENSOR_SAMPLE_PRIORITY, &(alt.update_task_handle)); + xTaskCreate(ADXL375::update_adxl375_task, "update_adxl375", 256, &adxl375, SENSOR_SAMPLE_PRIORITY, &(adxl375.update_task_handle)); + xTaskCreate(IIM42653::update_iim42653_task, "update_iim42653", 256, &iim42653, SENSOR_SAMPLE_PRIORITY, &(iim42653.update_task_handle)); + xTaskCreate(MMC5983MA::update_mmc5983ma_task, "update_mmc5983ma", 256, &mmc5983ma, SENSOR_SAMPLE_PRIORITY, &(mmc5983ma.update_task_handle)); - // Initialize Kalman Filter - kf = new kalman_filter(2, 1, 1, 0.01); - VectorXf state_vec(2); - MatrixXf state_cov(2, 2); - state_vec << altimeter.get_altitude_converted(), linear_acceleration.z(); - state_cov << 0.018, 0.0, 0.0, 0.0005; - kf->state_initialize(state_vec, state_cov); - ground_altitude = altimeter.get_altitude_converted(); + xTaskCreate(MS5607::ms5607_sample_handler, "ms5607_sample_handler", 256, &alt, SENSOR_EVENT_HANDLER_PRIORITY, &(alt.sample_handler_task_handle)); - altimeter.expose_buffer(&altimeter_buffer); - imu.expose_acceleration_buffer(&acceleration_buffer); - imu.expose_quaternion_buffer(&quaternion_buffer); + xTaskCreate(rocket_task, "rocket_task", 512, NULL, SENSOR_SAMPLE_PRIORITY, const_cast<TaskHandle_t *>(&rocket_task_handle)); +#if (DEBUG != 1) + xTaskCreate(kalman_task, "kalman_task", 512, NULL, KALMAN_TASK_PRIORITY, const_cast<TaskHandle_t *>(&kalman_task_handle)); + vTaskCoreAffinitySet(kalman_task_handle, 0x01); +#endif + vTaskCoreAffinitySet( alt.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( adxl375.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( iim42653.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( mmc5983ma.update_task_handle, 0x01 ); - sem_init(&sem, 1, 1); + vTaskCoreAffinitySet(rocket_task_handle, 0x01); - add_repeating_timer_us(-1000000 / DATA_RATE_HZ, &timer_callback, NULL, &data_timer); + vTaskCoreAffinitySet( alt.sample_handler_task_handle, 0x01 ); - multicore_launch_core1(logging_core); + xTaskCreate(logging_task, "logging", 256, NULL, LOGGING_PRIORITY, const_cast<TaskHandle_t *>(&logging_handle)); + vTaskCoreAffinitySet(logging_handle, 0x02); + + vTaskStartScheduler(); while (1) { tight_loop_contents(); } } -// PRIMARY THREAD RELATED FUNCTIONS AND CALLBACKS -//=============================================================================== - -bool timer_callback(repeating_timer_t *rt) { - sem_acquire_blocking(&sem); - imu.linear_acceleration(linear_acceleration); - imu.quaternion(quaternion); - - control(0) = linear_acceleration.z(); - measurement(0) = altimeter.get_altitude_converted(); - res = kf->run(control, measurement, 0.01f); - altitude = res(0); - velocity = res(1); - - - deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, (altitude - ground_altitude))), 100.0f)); - - switch(state) { - case PAD: - gpio_put(MOSFET_PIN, 0); - pwm.set_servo_percent(0); - deployment_percent = 0; - break; - case BOOST: - gpio_put(MOSFET_PIN, 1); - pwm.set_servo_percent(0); - deployment_percent = 0; - break; - case COAST: - gpio_put(MOSFET_PIN, 1); - pwm.set_servo_percent(deployment_percent); - break; - case APOGEE: - gpio_put(MOSFET_PIN, 1); - pwm.set_servo_percent(0); - deployment_percent = 0; - break; - case RECOVERY: - gpio_put(MOSFET_PIN, 1); - pwm.set_servo_percent(0); - deployment_percent = 0; - break; - case END: - gpio_put(MOSFET_PIN, 1); - pwm.set_servo_percent(0); - deployment_percent = 0; - break; - } - sem_release(&sem); - return true; +/****************************** LOGGING ***************************************/ +static void populate_log_entry(log_entry_t* log_entry) { + log_entry->time_us = time_us_64(); + + adc_select_input(4); + log_entry->temperature_chip = adc_read(); + log_entry->state = rocket_state; + log_entry->deploy_percent = deployment_percent; + log_entry->pressure = alt.get_pressure(); + log_entry->altitude = alt.get_altitude(); + log_entry->altitude_filt = altitude_filt; + log_entry->velocity_filt = velocity_filt; + log_entry->temperature_alt = alt.get_temperature(); + + log_entry->ax = iim42653.get_ax(); + log_entry->ay = iim42653.get_ay(); + log_entry->az = iim42653.get_az(); + log_entry->gx = iim42653.get_gx(); + log_entry->gy = iim42653.get_gy(); + log_entry->gz = iim42653.get_gz(); + + log_entry->mag_x = mmc5983ma.get_ax(); + log_entry->mag_y = mmc5983ma.get_ay(); + log_entry->mag_z = mmc5983ma.get_az(); + + log_entry->high_g_x = adxl375.get_ax(); + log_entry->high_g_y = adxl375.get_ay(); + log_entry->high_g_z = adxl375.get_az(); + + log_entry->drag_force = drag_force; + log_entry->apogee_prediction = apogee_prediction; + log_entry->desired_drag_force = desired_drag_force; + + log_entry->data0 = get_rand_32(); } -/** - * @brief Call back function for when rocket is on the pad - * - * @param gpio pin number of interrupt - * @param event_mask interrupt condition, value is set by PICO_SDK - * GPIO_IRQ_LEVEL_LOW = 0x1u, - * GPIO_IRQ_LEVEL_HIGH = 0x2u, - * GPIO_IRQ_EDGE_FALL = 0x4u, - * GPIO_IRQ_EDGE_RISE = 0x8u, - * @link https://www.raspberrypi.com/documentation/pico-sdk/hardware/gpio.html#ga6347e27da3ab34f1ea65b5ae16ab724f - */ -void pad_callback(uint gpio, uint32_t event_mask) { - sem_acquire_blocking(&sem); - altimeter.unset_threshold_altitude(INT1_PIN); - state = BOOST; - sem_release(&sem); - // start motor burn timer with boost transition function as callback - add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false); +static void logging_task(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / LOG_RATE_HZ); + + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + populate_log_entry(const_cast<log_entry_t *>(&log_entry)); + if (serial_data_output) { + print_log_entry(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry))); + stdio_flush(); + } + + if (use_circular_buffer) { + logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry))); + if (rocket_state != PAD) { + logger.flush_circular_buffer(true); + use_circular_buffer = false; + } + } else { + logger.write_memory(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)), false); + } + if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) { + xLastWakeTime = xTaskGetTickCount(); + } + } } +/****************************** LOGGING ***************************************/ -int64_t boost_callback(alarm_id_t id, void* user_data) { - // Configure accelerometer and/or altimeter to generate interrupt - // for when velocity is negative with this function as callback to - // transition to APOGEE - sem_acquire_blocking(&sem); - state = COAST; - populate_entry(); - write_entry(entry_buffer); - sem_release(&sem); - add_alarm_in_ms(1000, &coast_callback, NULL, false); - return 0; +/****************************** SERIAL CONSOLE ********************************/ +static void read_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + logger.read_memory(); + if (logging_handle != NULL) { + vTaskResume(logging_handle); + } } -int64_t coast_callback(alarm_id_t id, void* user_data) { - // Want to somehow immediately transition to RECOVERY from APOGEE (extremely short timer?) - if (velocity <= 0.0f) { - sem_acquire_blocking(&sem); - state = APOGEE; - populate_entry(); - write_entry(entry_buffer); - sem_release(&sem); - add_alarm_in_ms(1, &apogee_callback, NULL, false); +static void write_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + uint64_t start = time_us_64(); + log_entry_t log_entry; + populate_log_entry(&log_entry); + printf("\nWriting the following entry!\n"); + print_log_entry(reinterpret_cast<const uint8_t *>(&log_entry)); + if (use_circular_buffer) { + logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(&log_entry)); } else { - add_alarm_in_ms(250, &coast_callback, NULL, false); + logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true); + } + uint64_t end = time_us_64(); + printf("\nTook %" PRIu64 " us to write that entry!\n", (end - start)); + if (logging_handle != NULL) { + vTaskResume(logging_handle); } - return 0; } -int64_t apogee_callback(alarm_id_t id, void* user_data) { - state = RECOVERY; - // Set altimeter interrupt to occur for when rocket touches back to the ground - altimeter.set_threshold_altitude((ground_altitude + 10.0f), INT1_PIN, &recovery_callback); +static void erase_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + logger.erase_memory(); + if (logging_handle != NULL) { + vTaskResume(logging_handle); + } +} - sem_acquire_blocking(&sem); - populate_entry(); - write_entry(entry_buffer); - sem_release(&sem); - return 0; +static void show_cmd_func() { + serial_data_output = !serial_data_output; +} + +static void deploy_cmd_func() { + vTaskSuspend(rocket_task_handle); + printf("Enabling servo!\n"); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + vTaskDelay(pdMS_TO_TICKS(1000)); + printf("Setting servo to 80%\n"); + pwm.set_servo_percent(80); + vTaskDelay(pdMS_TO_TICKS(5000)); + printf("Setting servo to 0%\n"); + pwm.set_servo_percent(0); + vTaskDelay(pdMS_TO_TICKS(5000)); + printf("Setting servo to 80%\n"); + pwm.set_servo_percent(80); + vTaskDelay(pdMS_TO_TICKS(5000)); + printf("Setting servo to 0%\n"); + pwm.set_servo_percent(0); + vTaskDelay(pdMS_TO_TICKS(5000)); + printf("Disabling servo!\n"); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0); + vTaskResume(rocket_task_handle); } -void recovery_callback(uint gpio, uint32_t event_mask) { - // Essentially just a signal to stop logging data - sem_acquire_blocking(&sem); - state = END; - populate_entry(); - write_entry(entry_buffer); - sem_acquire_blocking(&sem); +#if (DEBUG == 1) +#include "ohio_test_data.h" +#endif + +static void kalman_cmd_func() { + static mf16* state_vector = kalman_get_state_vector(&kf); + printf("Perfoming Kalman Filter Test! Stand back!\n"); + printf("*******************************************\n"); + printf("\naltitude,velocity,acceleration,drag_force,apogee_prediction,desired_drag_force,desired_deployment\n"); + + fix16_t drag_force_l = 0; + fix16_t apogee_prediction_l = 0; + fix16_t desired_drag_force_l = 0; + fix16_t desired_deployment_l = 0; +#if (DEBUG == 1) + for (uint32_t i = 0; i < 7500; i++) { + kalman_update(altitude_test_data[i], fix16_mul(fix16_sub(acceleration_data[i], fix16_one), F16(9.81f))); + + if (i >= 693 && i <= 1632) { + drag_force_l = calculate_drag_force(fix16_from_int(80), state_vector->data[1][0]); + } else { + drag_force_l = calculate_drag_force(0, state_vector->data[1][0]); + } + apogee_prediction_l = predict_apogee(state_vector->data[0][0], state_vector->data[1][0], drag_force_l); + desired_drag_force_l = calculate_desired_drag_force(state_vector->data[0][0], state_vector->data[1][0]); + desired_deployment_l = calculate_deployment_percentage(desired_drag_force_l, state_vector->data[1][0]); + desired_deployment_l = fix16_clamp(desired_deployment_l, 0, fix16_from_int(100)); + printf("%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f\n", fix16_to_float(state_vector->data[0][0]), fix16_to_float(state_vector->data[1][0]), fix16_to_float(fix16_mul(acceleration_data[i], F16(9.81f))), fix16_to_float(drag_force_l), fix16_to_float(apogee_prediction_l), fix16_to_float(desired_drag_force_l), fix16_to_float(desired_deployment_l)); + stdio_flush(); + } +#else + kalman_update(0, 0); +#endif +} +/****************************** SERIAL CONSOLE ********************************/ + + +static void rocket_task(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ROCKET_TASK_RATE_HZ); + + vTaskDelay(pdMS_TO_TICKS(1000)); + ground_altitude = alt.get_altitude(); + + // Sign of life + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + pwm.set_servo_percent(0); + vTaskDelay(pdMS_TO_TICKS(1000)); + pwm.set_servo_percent(20); + vTaskDelay(pdMS_TO_TICKS(3000)); + pwm.set_servo_percent(0); + vTaskDelay(pdMS_TO_TICKS(3000)); + pwm.set_servo_percent(20); + vTaskDelay(pdMS_TO_TICKS(3000)); + pwm.set_servo_percent(0); + vTaskDelay(pdMS_TO_TICKS(3000)); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0); + + xTaskCreate(launch_event_handler_task, "launch_event_handler", 512, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&launch_event_handler_task_handle)); + vTaskCoreAffinitySet(launch_event_handler_task_handle, 0x01); + alt.set_threshold_altitude(ground_altitude + (threshold_altitude * ALTITUDE_SCALE), &launch_event_callback); + + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + + drag_force = calculate_drag_force(fix16_from_int(deployment_percent), velocity_filt); + apogee_prediction = predict_apogee(altitude_filt, velocity_filt, drag_force); + desired_drag_force = calculate_desired_drag_force(altitude_filt, velocity_filt); + desired_deployment = calculate_deployment_percentage(desired_drag_force, velocity_filt); + desired_deployment = fix16_clamp(desired_deployment, 0, fix16_from_int(100)); + + switch(rocket_state) { + case PAD: + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0); + break; + case BOOST: + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + break; + case COAST: + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + if (velocity_filt <= 0) { + rocket_state = APOGEE; + populate_log_entry(const_cast<log_entry_t *>(&log_entry)); + logger.write_memory(reinterpret_cast<const uint8_t *>(const_cast<log_entry_t *>(&log_entry)), false); + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + rocket_state = RECOVERY; + xTaskCreate(end_event_handler_task, "end_event_handler", 1024, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&end_event_handler_task_handle)); + vTaskCoreAffinitySet(end_event_handler_task_handle, 0x01); + add_alarm_in_ms(450000, end_event_callback, NULL, false); + } + deployment_percent = desired_deployment; + if ((alt.get_altitude() - ground_altitude)> (2895 * ALTITUDE_SCALE)) { + deployment_percent = 100; + } + pwm.set_servo_percent(deployment_percent); + break; + case APOGEE: + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + break; + case RECOVERY: + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + break; + case END: + deployment_percent = 0; + pwm.set_servo_percent(deployment_percent); + vTaskDelay(pdMS_TO_TICKS(1000)); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 0); + vTaskDelete(logging_handle); + vTaskDelete(end_event_handler_task_handle); + vTaskDelete(rocket_task_handle); + break; + } + + if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) { + xLastWakeTime = xTaskGetTickCount(); + } + } } -/** - * @brief Calculates the fitted Coeficient of Drag using the Surface Fit Model for the current rocket design. - * @param velocity Velocity - * @param altitude Altitude - * - * @return: Drag Coefficient (CD) - */ -float get_deploy_percent(float velocity, float altitude) { - // Lookup table (Data from 'dragSurfFit.m' for Surface Fit Model Formula) - float p00 = -8.498e+04; - float p10 = 924.4; - float p01 = 69.98; - float p20 = -3.62; - float p11 = -0.6196; - float p02 = -0.01897; - float p30 = 0.005983; - float p21 = 0.001756; - float p12 = 0.0001271; - float p03 = 1.693e-06; - float p40 = -3.451e-06; - float p31 = -1.582e-06; - float p22 = -2.004e-07; - float p13 = -7.476e-09; - - - /* MATLAB Code: - * return p00 + p10 * V + p01 * H + p20 * V ** 2 + \ - * p11 * V * H + p02 * H ** 2 + p30 * V ** 3 + \ - * p21 * V ** 2 * H + p12 * V * H ** 2 + p03 * H ** 3 + \ - * p40 * V ** 4 + p31 * V ** 3 * H + p22 * V ** 2 * H ** 2 + \ - * p13 * V * H ** 3 - */ - - return (p00 + p10 * velocity + p01 * altitude + p20 * std::pow(velocity, 2) + p11 * velocity * altitude + p02 * std::pow(altitude, 2) - + p30 * std::pow(velocity, 3) + p21 * std::pow(velocity, 2) * altitude + p12 * velocity * std::pow(altitude, 2) + p03 * std::pow(altitude, 3) - + p40 * std::pow(velocity, 4) + p31 * std::pow(velocity, 3) * altitude + p22 * std::pow(velocity, 2) * std::pow(altitude, 2) + p13 * velocity * std::pow(altitude, 3)); +static void kalman_task(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / KALMAN_TASK_RATE_HZ); + const fix16_t accel_scale = fix16_div(F16(9.81f), F16(S_IIM42653_ACCEL_SENSITIVITY_FACTOR)); + + mf16* state_vector = kalman_get_state_vector(&kf); + fix16_t altitude_agl = 0; + fix16_t vertical_accel = 1; + + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + taskENTER_CRITICAL(); + altitude_agl = fix16_div(fix16_from_int(alt.get_altitude() - ground_altitude), fix16_from_int(ALTITUDE_SCALE)); + vertical_accel = fix16_mul(fix16_sub(fix16_from_int(iim42653.get_az()), fix16_one), accel_scale); + kalman_update(altitude_agl, vertical_accel); + altitude_filt = state_vector->data[0][0]; + velocity_filt = state_vector->data[1][0]; + taskEXIT_CRITICAL(); + if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) { + xLastWakeTime = xTaskGetTickCount(); + } + } +} +static void launch_event_handler_task(void* pvParameters) { + const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 1000 ); + const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 ); + uint32_t ulEventsToProcess; + while (1) { + /* Wait to receive a notification sent directly to this task from the + interrupt service routine. */ + ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime ); + if( ulEventsToProcess != 0 ) { + /* To get here at least one event must have occurred. Loop here + until all the pending events have been processed */ + while( ulEventsToProcess > 0 ) { + vTaskSuspendAll(); + rocket_state = BOOST; + xTaskCreate(coast_event_handler_task, "coast_event_handler", 512, NULL, ROCKET_EVENT_HANDLER_PRIORITY, const_cast<TaskHandle_t *>(&coast_event_handler_task_handle)); + vTaskCoreAffinitySet(coast_event_handler_task_handle, 0x01); + add_alarm_in_ms(MOTOR_BURN_TIME, coast_event_callback, NULL, false); + vTaskDelete(launch_event_handler_task_handle); + launch_event_handler_task_handle = NULL; + xTaskResumeAll(); + } + } + } } -// LOGGING THREAD RELATED FUNCTIONS AND CALLBACKS -//=============================================================================== +static void coast_event_handler_task(void* pvParameters) { + const TickType_t xInterruptFrequency = pdMS_TO_TICKS( MOTOR_BURN_TIME ); + const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 ); + uint32_t ulEventsToProcess; + while (1) { + /* Wait to receive a notification sent directly to this task from the + interrupt service routine. */ + ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime ); + if( ulEventsToProcess != 0 ) { + /* To get here at least one event must have occurred. Loop here + until all the pending events have been processed */ + while( ulEventsToProcess > 0 ) { + vTaskSuspendAll(); + rocket_state = COAST; + vTaskDelete(coast_event_handler_task_handle); + coast_event_handler_task_handle = NULL; + xTaskResumeAll(); + } + } + } +} -void logging_core() { - add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_buffer_callback, NULL, &log_timer); - add_repeating_timer_us(-1000000 / HEART_RATE_HZ, &heartbeat_callback, NULL, &heartbeat_timer); +static void end_event_handler_task(void* pvParameters) { + const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 30000 ); + const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 500 ); + uint32_t ulEventsToProcess; while (1) { - tight_loop_contents(); + /* Wait to receive a notification sent directly to this task from the + interrupt service routine. */ + ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime ); + if( ulEventsToProcess != 0 ) { + /* To get here at least one event must have occurred. Loop here + until all the pending events have been processed */ + while( ulEventsToProcess > 0 ) { + rocket_state = END; + vTaskDelete(end_event_handler_task_handle); + end_event_handler_task_handle = NULL; + } + } } } -void populate_entry() { - absolute_time_t now = get_absolute_time(); - uint64_t now_us = to_us_since_boot(now); - uint32_t vel_bits = *((uint32_t *)&velocity); - entry_buffer[0] = now_us >> 56; - entry_buffer[1] = now_us >> 48; - entry_buffer[2] = now_us >> 40; - entry_buffer[3] = now_us >> 32; - entry_buffer[4] = now_us >> 24; - entry_buffer[5] = now_us >> 16; - entry_buffer[6] = now_us >> 8; - entry_buffer[7] = now_us; +int64_t launch_event_callback(alarm_id_t id, void* user_data) { + BaseType_t xHigherPriorityTaskWoken; + xHigherPriorityTaskWoken = pdFALSE; + // Defer ISR handling to separate handler within FreeRTOS context + vTaskNotifyGiveFromISR(launch_event_handler_task_handle, &xHigherPriorityTaskWoken ); + portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); + return 0; +} - adc_select_input(4); - uint16_t temperature = adc_read(); - entry_buffer[8] = ((*(uint8_t *)(&state)) << 4) | (temperature >> 8); - entry_buffer[9] = temperature; - - entry_buffer[10] = deployment_percent; - entry_buffer[11] = altimeter_buffer[0]; - entry_buffer[12] = altimeter_buffer[1]; - entry_buffer[13] = altimeter_buffer[2]; - entry_buffer[14] = vel_bits >> 24; - entry_buffer[15] = vel_bits >> 16; - entry_buffer[16] = vel_bits >> 8; - entry_buffer[17] = vel_bits; - entry_buffer[18] = acceleration_buffer[0]; - entry_buffer[19] = acceleration_buffer[1]; - entry_buffer[20] = acceleration_buffer[2]; - entry_buffer[21] = acceleration_buffer[3]; - entry_buffer[22] = acceleration_buffer[4]; - entry_buffer[23] = acceleration_buffer[5]; - entry_buffer[24] = quaternion_buffer[0]; - entry_buffer[25] = quaternion_buffer[1]; - entry_buffer[26] = quaternion_buffer[2]; - entry_buffer[27] = quaternion_buffer[3]; - entry_buffer[28] = quaternion_buffer[4]; - entry_buffer[29] = quaternion_buffer[5]; - entry_buffer[30] = quaternion_buffer[6]; - entry_buffer[31] = quaternion_buffer[7]; +int64_t coast_event_callback(alarm_id_t id, void* user_data) { + BaseType_t xHigherPriorityTaskWoken; + xHigherPriorityTaskWoken = pdFALSE; + // Defer ISR handling to separate handler within FreeRTOS context + vTaskNotifyGiveFromISR(coast_event_handler_task_handle, &xHigherPriorityTaskWoken ); + portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); + return 0; } -bool logging_buffer_callback(repeating_timer_t *rt) { - sem_acquire_blocking(&sem); - populate_entry(); - sem_release(&sem); - for (uint32_t i = 0; i < PACKET_SIZE; i++) { - pad_buffer[i + pad_buffer_offset] = entry_buffer[i]; - } - pad_buffer_offset += PACKET_SIZE; - pad_buffer_offset %= PAD_BUFFER_SIZE; - - if (state != PAD) { - uint32_t idx = ((pad_buffer_offset + PACKET_SIZE) % PAD_BUFFER_SIZE); - sem_acquire_blocking(&sem); - do { - write_entry(pad_buffer + idx); - idx += PACKET_SIZE; - idx %= PAD_BUFFER_SIZE; - } while (idx != pad_buffer_offset); - sem_release(&sem); - cancel_repeating_timer(&log_timer); - free(pad_buffer); - add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_flash_callback, NULL, &log_timer); - } - return true; +int64_t end_event_callback(alarm_id_t id, void* user_data) { + BaseType_t xHigherPriorityTaskWoken; + xHigherPriorityTaskWoken = pdFALSE; + // Defer ISR handling to separate handler within FreeRTOS context + vTaskNotifyGiveFromISR(end_event_handler_task_handle, &xHigherPriorityTaskWoken ); + portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); + return 0; } -bool logging_flash_callback(repeating_timer_t *rt) { - sem_acquire_blocking(&sem); - populate_entry(); - write_entry(entry_buffer); - sem_release(&sem); - if (state == END) { - cancel_repeating_timer(&log_timer); - } - return true; +fix16_t calculate_drag_force(fix16_t deployment_percentage, fix16_t vertical_velocity) { + static const fix16_t p00 = F16(125.f); + static const fix16_t p10 = F16(-3.286f); + static const fix16_t p01 = F16(-1.803f); + static const fix16_t p20 = F16(0.01675f); + static const fix16_t p11 = F16(0.02687f); + static const fix16_t p02 = F16(0.008441f); + + fix16_t term1 = fix16_mul(p10, deployment_percentage); + fix16_t term2 = fix16_mul(p01, vertical_velocity); + fix16_t term3 = fix16_mul(p20, fix16_sq(deployment_percentage)); + fix16_t term4 = fix16_mul(fix16_mul(p11, vertical_velocity), deployment_percentage); + fix16_t term5 = fix16_mul(fix16_mul(p02, vertical_velocity), vertical_velocity); + + fix16_t drag_force = fix16_add(p00, term1); + drag_force = fix16_add(drag_force, term2); + drag_force = fix16_add(drag_force, term3); + drag_force = fix16_add(drag_force, term4); + drag_force = fix16_add(drag_force, term5); + + return drag_force; } -bool heartbeat_callback(repeating_timer_t *rt) { - const bool sequence[] = {true, false, true, false, false}; - const uint8_t sequence_length = 5; +fix16_t predict_apogee(fix16_t altitude, fix16_t vertical_velocity, fix16_t drag_force) { + static const fix16_t gravity = F16(9.81f); + static const fix16_t mass = F16(21.8f); + + fix16_t nal_log_internal = fix16_div(gravity, fix16_add(gravity, fix16_div(drag_force, mass))); + fix16_t nal_log_scale = fix16_mul(fix16_mul(fix16_div(vertical_velocity, fix16_mul(F16(2), drag_force)), vertical_velocity), mass); + fix16_t apogee_prediction = fix16_sub(altitude, fix16_mul(nal_log_scale, fix16_log(nal_log_internal))); + return apogee_prediction; +} - bool led_status = sequence[led_counter]; - cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_status); - led_counter++; - led_counter %= sequence_length; - return true; +fix16_t calculate_deployment_percentage(fix16_t drag_force, fix16_t vertical_velocity) { + static const fix16_t p00 = F16(79.05f); + static const fix16_t p10 = F16(1.057f); + static const fix16_t p01 = F16(-1.049f); + static const fix16_t p20 = F16(-7.296e-5f); + static const fix16_t p11 = F16(-0.003321f); + static const fix16_t p02 = F16(0.002322f); + + + fix16_t term1 = fix16_mul(p10, drag_force); + fix16_t term2 = fix16_mul(p01, vertical_velocity); + fix16_t term3 = fix16_mul(fix16_mul(p20, drag_force), drag_force); + fix16_t term4 = fix16_mul(fix16_mul(p11, drag_force), vertical_velocity); + fix16_t term5 = fix16_mul(fix16_mul(p02, vertical_velocity), vertical_velocity); + + fix16_t deployment_percentage = fix16_add(p00, term1); + deployment_percentage = fix16_add(deployment_percentage, term2); + deployment_percentage = fix16_add(deployment_percentage, term3); + deployment_percentage = fix16_add(deployment_percentage, term4); + deployment_percentage = fix16_add(deployment_percentage, term5); + deployment_percentage = fix16_clamp(deployment_percentage, 0, fix16_from_int(100)); + return deployment_percentage; } +fix16_t calculate_desired_drag_force(fix16_t altitude, fix16_t vertical_velocity) { + static const fix16_t p00 = F16(-2.042e+01); + static const fix16_t p10 = F16(2.879e+01); + static const fix16_t p01 = F16(2.391e+02); + static const fix16_t p20 = F16(-1.265e+01); + static const fix16_t p11 = F16(-2.499e+02); + static const fix16_t p02 = F16(-1.063e+03); + static const fix16_t p30 = F16(1.774); + static const fix16_t p21 = F16(7.604e+01); + static const fix16_t p12 = F16(7.028e+02); + static const fix16_t p03 = F16(2.135e+03); + static const fix16_t p31 = F16(-6.349); + static const fix16_t p22 = F16(-1.049e+02); + static const fix16_t p13 = F16(-6.41e+02); + static const fix16_t p04 = F16(-1.604e+03); + + fix16_t altitude_km = fix16_div(altitude, fix16_from_int(1000)); + fix16_t vertical_velocity_km = fix16_div(vertical_velocity_km, fix16_from_int(1000)); + + fix16_t term01 = fix16_mul(p10, altitude_km); + fix16_t term02 = fix16_mul(p01, vertical_velocity_km); + fix16_t term03 = fix16_mul(fix16_mul(p20, altitude_km), altitude_km); + fix16_t term04 = fix16_mul(fix16_mul(p11, altitude_km), vertical_velocity_km); + fix16_t term05 = fix16_mul(fix16_mul(p02, vertical_velocity_km), vertical_velocity_km); + fix16_t term06 = fix16_mul(fix16_mul(fix16_mul(p30, altitude_km), altitude_km), altitude_km); + fix16_t term07 = fix16_mul(fix16_mul(fix16_mul(p21, altitude_km), altitude_km), vertical_velocity_km); + fix16_t term08 = fix16_mul(fix16_mul(fix16_mul(p12, altitude_km), vertical_velocity_km), vertical_velocity_km); + fix16_t term09 = fix16_mul(fix16_mul(fix16_mul(p03, vertical_velocity_km), vertical_velocity_km), vertical_velocity_km); + fix16_t term10 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p31, altitude_km), altitude_km), altitude_km), vertical_velocity_km); + fix16_t term11 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p22, altitude_km), altitude_km), vertical_velocity_km), vertical_velocity_km); + fix16_t term12 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p13, altitude_km), vertical_velocity_km), vertical_velocity_km), vertical_velocity_km); + fix16_t term13 = fix16_mul(fix16_mul(fix16_mul(fix16_mul(p04, vertical_velocity_km), vertical_velocity_km), vertical_velocity_km), vertical_velocity_km); + + fix16_t desired_drag_force = fix16_add(p00, term01); + desired_drag_force = fix16_add(desired_drag_force, term02); + desired_drag_force = fix16_add(desired_drag_force, term03); + desired_drag_force = fix16_add(desired_drag_force, term04); + desired_drag_force = fix16_add(desired_drag_force, term05); + desired_drag_force = fix16_add(desired_drag_force, term06); + desired_drag_force = fix16_add(desired_drag_force, term07); + desired_drag_force = fix16_add(desired_drag_force, term08); + desired_drag_force = fix16_add(desired_drag_force, term09); + desired_drag_force = fix16_add(desired_drag_force, term10); + desired_drag_force = fix16_add(desired_drag_force, term11); + desired_drag_force = fix16_add(desired_drag_force, term12); + desired_drag_force = fix16_add(desired_drag_force, term13); + + desired_drag_force = fix16_mul(desired_drag_force, fix16_from_int(1000)); + + return desired_drag_force; +} |