summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/CMakeLists.txt34
-rw-r--r--src/active_drag_system.cpp1131
-rw-r--r--src/adxl375.cpp60
-rw-r--r--src/altimeter.cpp145
-rw-r--r--src/heartbeat.cpp48
-rw-r--r--src/iim42653.cpp108
-rw-r--r--src/imu.cpp160
-rw-r--r--src/log_format.cpp66
-rw-r--r--src/mmc5983ma.cpp158
-rw-r--r--src/ms5607.cpp208
-rw-r--r--src/pwm.cpp6
-rw-r--r--src/serial.cpp134
-rw-r--r--src/spi_flash.c267
13 files changed, 1577 insertions, 948 deletions
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e372541..9aedf74 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,17 +1,27 @@
-add_executable(ads
- active_drag_system.cpp
- spi_flash.c
- imu.cpp
- pwm.cpp
- altimeter.cpp
- kalman_filter.cpp
+add_executable(active_drag_system
+ ${PROJECT_SOURCE_DIR}/src/active_drag_system.cpp
+ ${PROJECT_SOURCE_DIR}/src/ms5607.cpp
+ ${PROJECT_SOURCE_DIR}/src/adxl375.cpp
+ ${PROJECT_SOURCE_DIR}/src/iim42653.cpp
+ ${PROJECT_SOURCE_DIR}/src/mmc5983ma.cpp
+ ${PROJECT_SOURCE_DIR}/src/pwm.cpp
+ ${PROJECT_SOURCE_DIR}/src/log_format.cpp
+ ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp
+ ${PROJECT_SOURCE_DIR}/src/serial.cpp
)
-target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm hardware_adc pico_cyw43_arch_none ${Eigen_LIBRARIES})
-target_include_directories(ads PUBLIC ../include)
+pico_set_binary_type(active_drag_system copy_to_ram)
-pico_enable_stdio_usb(ads 0)
-pico_enable_stdio_uart(ads 0)
+target_link_libraries(active_drag_system pico_stdlib pico_logger pico_flash pico_rand pico_multicore pico_sync hardware_i2c hardware_adc hardware_timer hardware_pwm FreeRTOS-Kernel FreeRTOS-Kernel-Heap4 libfixmath libfixmatrix libfixkalman)
+target_include_directories(active_drag_system PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_compile_definitions(active_drag_system PRIVATE
+ USE_FREERTOS=1
+ # DEBUG=1
+ PICO_STDIO_STACK_BUFFER_SIZE=64 # use a small printf on stack buffer
+)
+
+pico_enable_stdio_usb(active_drag_system 1)
+pico_enable_stdio_uart(active_drag_system 0)
-pico_add_extra_outputs(ads)
+pico_add_extra_outputs(active_drag_system)
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;
+}
diff --git a/src/adxl375.cpp b/src/adxl375.cpp
new file mode 100644
index 0000000..5b18c42
--- /dev/null
+++ b/src/adxl375.cpp
@@ -0,0 +1,60 @@
+#include "adxl375.hpp"
+
+void ADXL375::initialize() {
+ //Configure power mode and ODR
+ buffer[0] = R_ADXL375_BW_RATE;
+ bw_rate.fields.LOW_POWER = B_ADXL375_BW_RATE_NORMAL_POWER_MODE;
+ bw_rate.fields.RATE = B_ADXL375_ODR_800_HZ;
+ buffer[1] = bw_rate.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+ //Configure data output format
+ buffer[0] = R_ADXL375_DATA_FORMAT;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 1, false);
+ //Some reserved fields are set to 1, read byte and *then* set desired bits
+ data_format.data = buffer[0];
+
+ buffer[0] = R_ADXL375_DATA_FORMAT;
+ data_format.fields.JUSTIFY = B_ADXL375_DATA_FORMAT_JUSTIFY_RIGHT;
+ data_format.fields.INT_INVERT = B_ADXL375_DATA_FORMAT_INTERRUPT_ACTIVE_HIGH;
+ buffer[1] = data_format.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+ //Set to measurement mode
+ buffer[0] = R_ADXL375_POWER_CTL;
+ power_ctl.fields.MEASURE = true;
+ buffer[1] = power_ctl.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+}
+
+void ADXL375::sample() {
+ //Read DATAX0 - DATAZ1 as a block
+ buffer[0] = R_ADXL375_DATAX0;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 6, false);
+
+ //Split buffer into individual fields
+ ax = ((int16_t) buffer[0]) | ((int16_t) buffer[1] << 8);
+ ay = ((int16_t) buffer[2]) | ((int16_t) buffer[3] << 8);
+ az = ((int16_t) buffer[4]) | ((int16_t) buffer[5] << 8);
+}
+
+#if (USE_FREERTOS == 1)
+void ADXL375::update_adxl375_task(void* pvParameters) {
+ TickType_t xLastWakeTime;
+ const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ADXL375_SAMPLE_RATE_HZ);
+
+ xLastWakeTime = xTaskGetTickCount();
+ while (1) {
+ vTaskDelayUntil(&xLastWakeTime, xFrequency);
+ taskENTER_CRITICAL();
+ ADXL375* adxl375 = (ADXL375 *) pvParameters;
+ adxl375->sample();
+ taskEXIT_CRITICAL();
+ if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+ xLastWakeTime = xTaskGetTickCount();
+ }
+ }
+}
+#endif
diff --git a/src/altimeter.cpp b/src/altimeter.cpp
deleted file mode 100644
index 9c47ceb..0000000
--- a/src/altimeter.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-#include "altimeter.hpp"
-#include "hardware/gpio.h"
-
-altimeter::altimeter(i2c_inst_t* inst, uint8_t addr) {
- this->inst = inst;
- this->addr = addr;
-}
-
-void altimeter::initialize() {
- // Select control register(0x26)
- // Active mode, OSR = 16, altimeter mode(0xB8)
- this->buffer[0] = 0x26;
- this->buffer[1] = 0x89;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-}
-
-void altimeter::initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) {
- this->initialize();
-
- // Below configures the interrupt for the first transition from PAD to BOOST
- // Initial Reading
-
- sleep_ms(1000);
-
- float altitude = 0.0f;
-
- while (altitude == 0.0f) {
- altitude = this->get_altitude_converted();
- }
-
- threshold_altitude += altitude; // 30 meters above ground
-
- // Select control register 3 (0x28)
- // Set bot interrupt pins to active low and enable internal pullups
- this->buffer[0] = 0x28;
- this->buffer[1] = 0x01;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select pressure target MSB register(0x16)
- // Set altitude target to 30 meters above ground altitude
- this->buffer[0] = 0x16;
- this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select pressure target LSB register(0x17)
- // Set altitude target to 30 meters above ground altitude
- this->buffer[0] = 0x17;
- this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)));
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select interrupt enable register (0x29)
- // Set interrupt enabled for altitude threshold(0x08)
- this->buffer[0] = 0x29;
- this->buffer[1] = 0x08;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select interrupt this->bufferuration register (0x2A)
- // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
- this->buffer[0] = 0x2A;
- this->buffer[1] = 0x08;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback);
- // End of configuration of interrupt for first transition from PAD to BOOST
-}
-
-void altimeter::set_threshold_altitude(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) {
- float altitude = 0.0f;
-
- while (altitude == 0.0f) {
- altitude = get_altitude_converted();
- }
-
- threshold_altitude += altitude;
-
- // Select control register 3 (0x28)
- // Set bot interrupt pins to active low and enable internal pullups
- this->buffer[0] = 0x28;
- this->buffer[1] = 0x01;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select pressure target MSB register(0x16)
- // Set altitude target to 30 meters above ground altitude
- this->buffer[0] = 0x16;
- this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select pressure target LSB register(0x17)
- // Set altitude target to provided threshold altitude
- this->buffer[0] = 0x17;
- this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)));
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select interrupt enable register (0x29)
- // Set interrupt enabled for altitude threshold(0x08)
- this->buffer[0] = 0x29;
- this->buffer[1] = 0x08;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select interrupt this->bufferuration register (0x2A)
- // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
- this->buffer[0] = 0x2A;
- this->buffer[1] = 0x08;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback);
- // End of configuration of interrupt for first transition from PAD to BOOST
-}
-
-void altimeter::unset_threshold_altitude(uint8_t interrupt_pin) {
- gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, false, NULL);
-
- // Select interrupt enable register (0x29)
- // Set interrupt enabled for altitude threshold(0x08)
- this->buffer[0] = 0x29;
- this->buffer[1] = 0x00;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-
- // Select interrupt configuration register (0x2A)
- // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
- this->buffer[0] = 0x2A;
- this->buffer[1] = 0x00;
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
-}
-
-float altimeter::get_altitude_converted() {
- uint8_t reg = 0x01;
- i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
- i2c_read_blocking(this->inst, this->addr, this->altitude_buffer, 4, false);
- // Exactly how MPL3115A2 datasheet says to retrieve altitude
- float altitude = (float) ((int16_t) ((this->altitude_buffer[0] << 8) | this->altitude_buffer[1])) + (float) (this->altitude_buffer[2] >> 4) * 0.0625;
- return altitude;
-}
-
-void altimeter::get_altitude_raw(uint8_t* buffer) {
- uint8_t reg = 0x01;
- i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
- i2c_read_blocking(this->inst, this->addr, buffer, 3, false);
-}
-
-uint32_t altimeter::expose_buffer(uint8_t** buffer) {
- *buffer = this->altitude_buffer;
- return sizeof(this->altitude_buffer);
-}
-
diff --git a/src/heartbeat.cpp b/src/heartbeat.cpp
new file mode 100644
index 0000000..e627935
--- /dev/null
+++ b/src/heartbeat.cpp
@@ -0,0 +1,48 @@
+#include "heartbeat.hpp"
+
+static int led_pin;
+
+#if (USE_FREERTOS != 1)
+static repeating_timer_t heartbeat_timer;
+
+static bool heartbeat_callback(repeating_timer_t *rt) {
+ static volatile uint8_t led_counter = 0;
+ const bool sequence[] = {true, false, true, false, false};
+ const uint8_t sequence_length = 5;
+ bool led_status = sequence[led_counter];
+ gpio_put(led_pin, led_status);
+ led_counter++;
+ led_counter %= sequence_length;
+ return true;
+}
+#endif
+
+void heartbeat_initialize(int gpio) {
+ led_pin = gpio;
+ gpio_init(led_pin);
+ gpio_set_dir(led_pin, GPIO_OUT);
+ gpio_put(led_pin, 0);
+#if (USE_FREERTOS != 1)
+ add_repeating_timer_us(-1000000 / HEART_RATE_HZ, &heartbeat_callback, NULL, &heartbeat_timer);
+#endif
+}
+
+
+#if (USE_FREERTOS == 1)
+void heartbeat_task( void *pvParameters ) {
+ TickType_t xLastWakeTime;
+ const TickType_t xFrequency = pdMS_TO_TICKS(1000 / HEART_RATE_HZ);
+ static volatile uint8_t led_counter = 0;
+ const bool sequence[] = {true, false, true, false, false};
+ const uint8_t sequence_length = 5;
+
+ xLastWakeTime = xTaskGetTickCount();
+ while (1) {
+ vTaskDelayUntil(&xLastWakeTime, xFrequency);
+ bool led_status = sequence[led_counter];
+ gpio_put(PICO_DEFAULT_LED_PIN, led_status);
+ led_counter++;
+ led_counter %= sequence_length;
+ }
+}
+#endif
diff --git a/src/iim42653.cpp b/src/iim42653.cpp
new file mode 100644
index 0000000..d9b96d4
--- /dev/null
+++ b/src/iim42653.cpp
@@ -0,0 +1,108 @@
+#include "iim42653.hpp"
+
+int16_t IIM42653::sat_sub(int16_t a, int16_t b) {
+ int32_t result = (int32_t) a - (int32_t) b;
+ if (result < INT16_MIN) {
+ result = INT16_MIN;
+ }
+ if (result > INT16_MAX) {
+ result = INT16_MAX;
+ }
+ return (int16_t) result;
+};
+
+//Startup routine, initialize GPIO clock output
+void IIM42653::initialize() {
+ //Enable 40kHz clock output on GPIO 23
+ clock_gpio_init(MICRO_DEFAULT_CLK_OUTPUT, IIM42653_CLOCK_SOURCE_SYSTEM, IIM42653_CLOCK_DIVISOR);
+ sleep_ms(50); //Allow time for clock output to stabilize
+
+ //Configure Gyroscope FSR and ODR
+ buffer[0] = R_IIM42653_GYRO_CONFIG0;
+ gyro_config0.fields.GYRO_ODR = B_IIM42653_GYRO_CONFIG0_ODR_500HZ;
+ gyro_config0.fields.GYRO_UI_FS_SEL = B_IIM42653_GYRO_CONFIG0_FSR_4000DPS;
+ buffer[1] = gyro_config0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+ //Configure Accelerometer FSR and ODR
+ buffer[0] = R_IIM42653_ACCEL_CONFIG0;
+ accel_config0.fields.ACCEL_ODR = B_IIM42653_ACCEL_CONFIG0_ODR_500HZ;
+ accel_config0.fields.ACCEL_UI_FS_SEL = B_IIM42653_ACCEL_CONFIG0_FSR_32G;
+ buffer[1] = accel_config0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+ //Enable Gyroscope, Accelerometer, and Thermocouple
+ buffer[0] = R_IIM42653_PWR_MGMT0;
+ pwr_mgmt0.fields.ACCEL_MODE = B_IIM42653_PWR_MGMT0_ACCEL_MODE_LOW_NOISE;
+ pwr_mgmt0.fields.GYRO_MODE = B_IIM42653_PWR_MGMT0_GYRO_MODE_LOW_NOISE;
+ pwr_mgmt0.fields.TEMP_DIS = B_IIM42653_PWR_MGMT0_TEMP_ENABLE;
+ buffer[1] = pwr_mgmt0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ sleep_ms(10); //Datasheet instructs 200us wait after changing sensor power modes
+
+ //Calibrate gyro bias once all sensors are initialized
+ calibrate_gyro();
+}
+
+//Read all 12 data registers at once and format them into their internal fields
+void IIM42653::sample() {
+ //Read ACCEL_DATA_X1_UI - GYRO_DATA_Z0_UI as a block
+ buffer[0] = R_IIM42653_ACCEL_DATA_X1_UI;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 12, false);
+
+ //Split buffer into individial fields
+ ax = ((int16_t)buffer[1]) | (((int16_t)buffer[0]) << 8);
+ ay = ((int16_t)buffer[3]) | (((int16_t)buffer[2]) << 8);
+ az = ((int16_t)buffer[5]) | (((int16_t)buffer[4]) << 8);
+ raw_gx = ((int16_t)buffer[7]) | (((int16_t)buffer[6]) << 8);
+ raw_gy = ((int16_t)buffer[9]) | (((int16_t)buffer[8]) << 8);
+ raw_gz = ((int16_t)buffer[11]) | (((int16_t)buffer[10]) << 8);
+}
+void IIM42653::apply_gyro_offset() {
+ gx = sat_sub(raw_gx, bias_gx);
+ gy = sat_sub(raw_gy, bias_gy);
+ gz = sat_sub(raw_gz, bias_gz);
+}
+
+//Request certain number of gyroscope readings, and set the gyro bias values to the averages of the readings
+void IIM42653::calibrate_gyro() {
+ //Initialize buffers
+ int64_t g_x = 0;
+ int64_t g_y = 0;
+ int64_t g_z = 0;
+
+ //Request configured number of samples and l
+ for (int64_t i = 0; i < n_gyro_bias_readings; i++) {
+ sample();
+ g_x += raw_gx;
+ g_y += raw_gy;
+ g_z += raw_gz;
+ sleep_ms(5);
+ }
+
+ //Set bias values to the average readings during the calibration period
+ bias_gx = g_x/n_gyro_bias_readings;
+ bias_gy = g_y/n_gyro_bias_readings;
+ bias_gz = g_z/n_gyro_bias_readings;
+}
+
+#if ( USE_FREERTOS == 1 )
+void IIM42653::update_iim42653_task(void* pvParameters) {
+ TickType_t xLastWakeTime;
+ const TickType_t xFrequency = pdMS_TO_TICKS(1000 / IIM42653_SAMPLE_RATE_HZ);
+
+ xLastWakeTime = xTaskGetTickCount();
+ while (1) {
+ vTaskDelayUntil(&xLastWakeTime, xFrequency);
+ taskENTER_CRITICAL();
+ IIM42653* iim42653 = (IIM42653*) pvParameters;
+ iim42653->sample();
+ iim42653->apply_gyro_offset();
+ taskEXIT_CRITICAL();
+ if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+ xLastWakeTime = xTaskGetTickCount();
+ }
+ }
+}
+#endif
diff --git a/src/imu.cpp b/src/imu.cpp
deleted file mode 100644
index 4a81042..0000000
--- a/src/imu.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-#include "imu.hpp"
-
-imu::imu(i2c_inst_t* inst, uint8_t addr, uint8_t id, imu_opmode_t mode) {
- this->inst = inst;
- this->addr = addr;
- this->id = id;
- this->mode = mode;
-}
-
-void imu::reset() {
- this->buffer[0] = SYS_TRIGGER;
- this->buffer[1] = 0x20; // Reset system
- i2c_write_blocking(this->inst, this->addr, buffer, 2, true);
- sleep_ms(1000); // Wait 650ms for the sensor to reset
-}
-
-void imu::initialize() {
- sleep_ms(1000); // Wait 650ms for the sensor to reset
-
- uint8_t chip_id_addr = CHIP_ID;
- uint8_t read_id = 0x00;
- while (read_id != this->id) {
- i2c_write_blocking(this->inst, this->addr, &chip_id_addr, 1, false);
- i2c_read_blocking(this->inst, this->addr, &read_id, 1, false);
- sleep_ms(100);
- }
-
- // Use internal oscillator
- this->buffer[0] = SYS_TRIGGER;
- this->buffer[1] = 0x40; // Set to use internal oscillator
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(50);
-
- // Reset all interrupt status bits
- this->buffer[0] = SYS_TRIGGER;
- this->buffer[1] = 0x01; // Reset interrupt status
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(50);
-
- // Set to normal power mode
- this->buffer[0] = POWER_MODE;
- this->buffer[1] = 0x00; // Normal power mode
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(50);
-
- // Default Axis Config
- this->buffer[0] = AXIS_MAP_CONFIG;
- this->buffer[1] = 0x24; // P1=Z, P2=Y, P3=X
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(50);
-
- // Default Axis Sign
- this->buffer[0] = AXIS_MAP_SIGN;
- this->buffer[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(50);
-
- // Set units to m/s^2
- this->buffer[0] = UNIT_SELECTION;
- this->buffer[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
- sleep_ms(200);
-
- uint8_t sensor_offsets[19];
- sensor_offsets[0] = ACCELERATION_OFFSET_X_LSB;
- sensor_offsets[1] = 0x00;
- sensor_offsets[2] = 0x00;
- sensor_offsets[3] = 0x00;
- sensor_offsets[4] = 0x00;
- sensor_offsets[5] = 0x00;
- sensor_offsets[6] = 0x00;
- sensor_offsets[7] = 0x00;
- sensor_offsets[8] = 0x00;
- sensor_offsets[9] = 0x00;
- sensor_offsets[10] = 0x00;
- sensor_offsets[11] = 0x00;
- sensor_offsets[12] = 0x00;
- sensor_offsets[13] = 0x00;
- sensor_offsets[14] = 0x00;
- sensor_offsets[15] = 0x00;
- sensor_offsets[16] = 0x00;
- sensor_offsets[17] = 0x00;
- sensor_offsets[18] = 0x00;
- i2c_write_blocking(this->inst, this->addr, sensor_offsets, 19, true);
- sleep_ms(200);
-
- // The default operation mode after power-on is CONFIG
- // Set to desired mode
- this->buffer[0] = OPERATION_MODE;
- this->buffer[1] = this->mode; // NDOF
- i2c_write_blocking(this->inst, this->addr, this->buffer, 2, false);
- sleep_ms(100);
-}
-
-void imu::calibration_status(calibration_status_t* status) {
- read_register(CALIBRATION_STATUS, 1, this->buffer);
- status->mag = ((this->buffer[0] & 0b00000011) >> 0);
- status->accel = ((this->buffer[0] & 0b00001100) >> 2);
- status->gyro = ((this->buffer[0] & 0b00110000) >> 4);
- status->sys = ((this->buffer[0] & 0b11000000) >> 6);
-}
-
-void imu::linear_acceleration(Eigen::Vector3f& vec) {
- read_register(LINEAR_ACCELERATION_X_LSB, 6, this->accel_buffer);
- int16_t x, y, z;
- x = y = z = 0;
- x = ((int16_t)this->accel_buffer[0]) | (((int16_t)this->accel_buffer[1]) << 8);
- y = ((int16_t)this->accel_buffer[2]) | (((int16_t)this->accel_buffer[3]) << 8);
- z = ((int16_t)this->accel_buffer[4]) | (((int16_t)this->accel_buffer[5]) << 8);
- vec(0) = ((float)x) / 100.0;
- vec(1) = ((float)y) / 100.0;
- vec(2) = ((float)z) / 100.0;
-}
-
-void imu::quaternion(Eigen::Vector4f& vec) {
- read_register(QUATERNION_W_LSB, 8, this->quat_buffer);
- int16_t w, x, y, z;
- w = x = y = z = 0;
- w = ((int16_t)this->quat_buffer[0]) | (((int16_t)this->quat_buffer[1]) << 8);
- x = ((int16_t)this->quat_buffer[2]) | (((int16_t)this->quat_buffer[3]) << 8);
- y = ((int16_t)this->quat_buffer[4]) | (((int16_t)this->quat_buffer[5]) << 8);
- z = ((int16_t)this->quat_buffer[6]) | (((int16_t)this->quat_buffer[7]) << 8);
- vec(0) = ((float)w) / 16384.0;
- vec(1) = ((float)x) / 16384.0;
- vec(2) = ((float)y) / 16384.0;
- vec(3) = ((float)z) / 16384.0;
-}
-
-void imu::quaternion_euler(Eigen::Vector3f& angles, Eigen::Vector4f& quat) {
- // roll (x-axis rotation)
- float sinr_cosp = 2 * (quat(0) * quat(1) + quat(2) * quat(3));
- float cosr_cosp = 1 - 2 * (quat(1) * quat(1) + quat(2) * quat(2));
- angles(0) = Eigen::numext::atan2(sinr_cosp, cosr_cosp);
-
- // pitch (y-axis rotation)
- float sinp = Eigen::numext::sqrt(1 + 2 * (quat(0) * quat(2) - quat(1) * quat(3)));
- float cosp = Eigen::numext::sqrt(1 - 2 * (quat(0) * quat(2) - quat(1) * quat(3)));
- angles(1) = 2 * Eigen::numext::atan2(sinp, cosp) - M_PI / 2;
-
- // yaw (z-axis rotation)
- float siny_cosp = 2 * (quat(0) * quat(3) + quat(1) * quat(2));
- float cosy_cosp = 1 - 2 * (quat(2) * quat(2) + quat(3) * quat(3));
- angles(2) = Eigen::numext::atan2(siny_cosp, cosy_cosp);
-}
-
-uint32_t imu::expose_acceleration_buffer(uint8_t** buffer) {
- *buffer = this->accel_buffer;
- return sizeof(this->accel_buffer);
-}
-
-uint32_t imu::expose_quaternion_buffer(uint8_t** buffer) {
- *buffer = this->quat_buffer;
- return sizeof(this->quat_buffer);
-}
-
-void imu::read_register(uint8_t reg, size_t len, uint8_t* buffer) {
- i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
- i2c_read_blocking(this->inst, this->addr, buffer, len, false);
-}
-
diff --git a/src/log_format.cpp b/src/log_format.cpp
new file mode 100644
index 0000000..5d9d275
--- /dev/null
+++ b/src/log_format.cpp
@@ -0,0 +1,66 @@
+#include "log_format.hpp"
+
+void print_log_entry(const uint8_t* entry) {
+ static bool first_call = true;
+
+ if (first_call) {
+ printf("time_us,state,temperature_chip_celsius,deployment_percent,pressure_mb,altitude,altitude_filt,velocity_filt,temperature_alt_celsius,mid_imu_ax,mid_imu_ay,mid_imu_az,mid_imu_gx,mid_imu_gy,mid_imu_gz,mag_x,mag_y,mag_z,high_g_x,high_g_y,high_g_z,drag_force,apogee_prediction,desired_drag_force\r\n");
+ first_call = false;
+ }
+
+ const log_entry_t* packet = reinterpret_cast<const log_entry_t *>(entry);
+ printf("%" PRIu64 ",", packet->time_us);
+ state_t state = (state_t) packet->state;
+ switch (state) {
+ case PAD:
+ printf("PAD");
+ break;
+ case BOOST:
+ printf("BOOST");
+ break;
+ case COAST:
+ printf("COAST");
+ break;
+ case APOGEE:
+ printf("APOGEE");
+ break;
+ case RECOVERY:
+ printf("RECOVERY");
+ break;
+ case END:
+ printf("END");
+ break;
+ }
+ printf(",");
+ const float conversionFactor = 3.3f / (1 << 12);
+ float tempC = 27.0f - (((float)(packet->temperature_chip) * conversionFactor) - 0.706f) / 0.001721f;
+ printf("%4.2f,", tempC);
+ printf("%d,", packet->deploy_percent);
+ printf("%4.2f,", ((float) packet->pressure) / PRESSURE_SCALE_F);
+ printf("%4.2f,", ((float) packet->altitude) / ALTITUDE_SCALE_F);
+ printf("%4.2f,", (fix16_to_float(packet->altitude_filt)));
+ printf("%4.2f,", (fix16_to_float(packet->velocity_filt)));
+ printf("%4.2f,", ((float) packet->temperature_alt) / TEMPERATURE_SCALE_F);
+
+ printf("%4.2f,", IIM42653::scale_accel(packet->ax));
+ printf("%4.2f,", IIM42653::scale_accel(packet->ay));
+ printf("%4.2f,", IIM42653::scale_accel(packet->az));
+
+ printf("%4.2f,", IIM42653::scale_gyro(packet->gx));
+ printf("%4.2f,", IIM42653::scale_gyro(packet->gy));
+ printf("%4.2f,", IIM42653::scale_gyro(packet->gz));
+
+ printf("%" PRIi16 ",", packet->mag_x);
+ printf("%" PRIi16 ",", packet->mag_y);
+ printf("%" PRIi16 ",", packet->mag_z);
+
+ printf("%4.2f,", ADXL375::scale(packet->high_g_x));
+ printf("%4.2f,", ADXL375::scale(packet->high_g_y));
+ printf("%4.2f,", ADXL375::scale(packet->high_g_z));
+ printf("%4.2f,", (fix16_to_float(packet->drag_force)));
+ printf("%4.2f,", (fix16_to_float(packet->apogee_prediction)));
+ printf("%4.2f,", (fix16_to_float(packet->desired_drag_force)));
+ printf("\r\n");
+ stdio_flush();
+}
+
diff --git a/src/mmc5983ma.cpp b/src/mmc5983ma.cpp
new file mode 100644
index 0000000..45ef94a
--- /dev/null
+++ b/src/mmc5983ma.cpp
@@ -0,0 +1,158 @@
+#include "mmc5983ma.hpp"
+
+int16_t MMC5983MA::sat_sub(int16_t a, int16_t b) {
+ int32_t result = (int32_t) a - (int32_t) b;
+ if (result < INT16_MIN) {
+ result = INT16_MIN;
+ }
+ if (result > INT16_MAX) {
+ result = INT16_MAX;
+ }
+ return (int16_t) result;
+};
+
+void MMC5983MA::initialize() {
+ // Restart device prior to configuration
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
+ internal_ctl1.fields.RESTART = true;
+ buffer[1] = internal_ctl1.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ internal_ctl1.data = 0;
+
+ sleep_ms(100);
+
+ calibrate();
+
+ //Configure decimation filter bandwidth for 200 Hz
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
+ internal_ctl1.fields.BANDWIDTH = B_MMC5983MA_BANDWIDTH_200HZ;
+ buffer[1] = internal_ctl1.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+
+ //Configure and enable continuous measurement mode for 200 Hz
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL2;
+ internal_ctl2.fields.CONTINUOUS_MODE_ENABLE = true;
+ internal_ctl2.fields.CONTINUOUS_MODE_FREQ = B_MMC5983_CONTINUOUS_MODE_FREQ_200HZ;
+ internal_ctl2.fields.PERIODIC_SET_ENABLE = false;
+ internal_ctl2.fields.PERIODIC_SET_RATE = B_MMC5983_PERIODIC_SET_RATE_MEAS_TIMES_1000;
+ buffer[1] = internal_ctl2.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ printf("MMC5983MA Initialized!\n");
+}
+
+void MMC5983MA::sample() {
+ buffer[0] = R_MMC5983MA_XOUT0;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 6, false);
+
+ raw_x = (int16_t) ((((uint16_t) buffer[0] << 8) | buffer[1]) - 32768);
+ raw_y = (int16_t) ((((uint16_t) buffer[2] << 8) | buffer[3]) - 32768);
+ raw_z = (int16_t) ((((uint16_t) buffer[4] << 8) | buffer[5]) - 32768);
+}
+
+void MMC5983MA::apply_offset() {
+ ax = sat_sub(raw_x, offset_x);
+ ay = sat_sub(raw_y, offset_y);
+ az = sat_sub(raw_z, offset_z);
+}
+
+void MMC5983MA::calibrate() {
+ sleep_ms(100);
+
+ printf("MMC5983MA Calibration | Set command");
+ // Take measurement after SET command completes
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+ internal_ctl0.fields.SET_CMD = true;
+ buffer[1] = internal_ctl0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ internal_ctl0.data = 0;
+
+ sleep_ms(100);
+ printf(" | Measurement Request");
+
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+ internal_ctl0.fields.TAKE_MAG_MEAS = true;
+ buffer[1] = internal_ctl0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ internal_ctl0.data = 0;
+ printf(" | Request sent, awaiting completion...\n");
+
+ size_t counter = 0;
+ while (!dev_status.fields.MEAS_M_DONE) {
+ sleep_ms(1);
+ buffer[0] = R_MMC5983MA_DEV_STATUS;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 1, false);
+ dev_status.data = buffer[0];
+ printf("W | ");
+ counter++;
+ if (counter == 20) {
+ printf("\n");
+ counter = 0;
+ }
+ }
+ printf("\n\nMeasurement taken!");
+
+ sample();
+ printf(" | Sample taken!");
+
+ int16_t set_ax = raw_x, set_ay = raw_y, set_az = raw_z;
+
+ // Take measurement after RESET command completes
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+ internal_ctl0.fields.RESET_CMD = true;
+ buffer[1] = internal_ctl0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ internal_ctl0.data = 0;
+
+ sleep_ms(100);
+
+ buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
+ internal_ctl0.fields.TAKE_MAG_MEAS = true;
+ buffer[1] = internal_ctl0.data;
+ i2c_write_blocking(i2c, addr, buffer, 2, false);
+ internal_ctl0.data = 0;
+
+ while (!dev_status.fields.MEAS_M_DONE) {
+ sleep_ms(1);
+ buffer[0] = R_MMC5983MA_DEV_STATUS;
+ i2c_write_blocking(i2c, addr, buffer, 1, true);
+ i2c_read_blocking(i2c, addr, buffer, 1, false);
+ dev_status.data = buffer[0];
+ }
+
+ sample();
+
+ int16_t reset_ax = raw_x, reset_ay = raw_y, reset_az = raw_z;
+
+ sleep_ms(100);
+
+ // Average the two measurements taken and assign them as offsets
+ offset_x = (int16_t) (((int32_t) set_ax + (int32_t) reset_ax) / 2);
+ offset_y = (int16_t) (((int32_t) set_ay + (int32_t) reset_ay) / 2);
+ offset_z = (int16_t) (((int32_t) set_az + (int32_t) reset_az) / 2);
+
+#if ( DEBUG == 1 )
+ printf("MMC5983MA: Calibrated!\n\toffset_x: %" PRIi16 "\n\toffset_y: %" PRIi16 "\n\toffset_z: %" PRIi16 "\n", offset_x, offset_y, offset_z);
+#endif
+}
+
+#if (USE_FREERTOS == 1)
+void MMC5983MA::update_mmc5983ma_task(void* pvParameters) {
+ TickType_t xLastWakeTime;
+ const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MMC5983_SAMPLE_RATE_HZ);
+
+ xLastWakeTime = xTaskGetTickCount();
+ while (1) {
+ vTaskDelayUntil(&xLastWakeTime, xFrequency);
+ taskENTER_CRITICAL();
+ MMC5983MA* mag = (MMC5983MA *) pvParameters;
+ mag->sample();
+ mag->apply_offset();
+ taskEXIT_CRITICAL();
+ if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
+ xLastWakeTime = xTaskGetTickCount();
+ }
+ }
+}
+#endif
diff --git a/src/ms5607.cpp b/src/ms5607.cpp
new file mode 100644
index 0000000..c093189
--- /dev/null
+++ b/src/ms5607.cpp
@@ -0,0 +1,208 @@
+#include "ms5607.hpp"
+
+static const int32_t altitude_table[] = {
+#include "altitude-pa.h"
+};
+
+#define ALT_SCALE (1 << ALT_SHIFT)
+#define ALT_MASK (ALT_SCALE - 1)
+
+void MS5607::initialize() {
+ sample_state = NOT_SAMPLING;
+
+ alarm_pool_init_default();
+
+ ms5607_cmd cmd;
+ cmd.data = RESET_COMMAND;
+
+ ms5607_write_cmd(&cmd);
+
+ sleep_ms(500);
+
+ cmd.data = 0;
+ cmd.fields.PROM = true;
+ cmd.fields.PROM2 = true;
+
+ cmd.fields.ADDR_OSR = PROM_CALIBRATION_COEFFICENT_1_ADDR;
+
+ for (uint8_t prom_addr = PROM_CALIBRATION_COEFFICENT_1_ADDR; prom_addr <= PROM_CALIBRATION_COEFFICENT_6_ADDR; prom_addr++) {
+ sleep_ms(100);
+ cmd.fields.ADDR_OSR = prom_addr;
+ ms5607_write_cmd(&cmd);
+ i2c_read_blocking(i2c, addr, buffer, 2, false);
+
+ prom[prom_addr - 1] = static_cast<uint16_t>((((uint16_t) buffer[0]) << 8) | ((uint16_t) buffer[1]));
+ }
+}
+
+void MS5607::ms5607_write_cmd(ms5607_cmd* cmd) {
+ i2c_write_blocking(i2c, addr, (uint8_t *) cmd, 1, false);
+}
+
+void MS5607::ms5607_start_sample() {
+ if (sample_state == NOT_SAMPLING) {
+ sample_state = PRESSURE_CONVERT;
+ ms5607_sample();
+ }
+}
+
+#if (USE_FREERTOS == 1)
+void MS5607::ms5607_sample_handler(void* pvParameters) {
+ /* xMaxExpectedBlockTime is set to be a little longer than the maximum
+ expected time between events. */
+ const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 1000 / (MS5607_SAMPLE_RATE_HZ * 2) );
+ const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 1 );
+ 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 ) {
+ MS5607* alt = (MS5607 *) pvParameters;
+ taskENTER_CRITICAL();
+ alt->ms5607_sample();
+ taskEXIT_CRITICAL();
+ ulEventsToProcess--;
+ }
+ }
+ }
+
+}
+
+void MS5607::update_ms5607_task(void* pvParameters) {
+ TickType_t xLastWakeTime;
+ const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MS5607_SAMPLE_RATE_HZ);
+
+ xLastWakeTime = xTaskGetTickCount();
+ while (1) {
+ vTaskDelayUntil(&xLastWakeTime, xFrequency);
+ MS5607* alt = (MS5607 *) pvParameters;
+ taskENTER_CRITICAL();
+ alt->ms5607_start_sample();
+ taskEXIT_CRITICAL();
+ }
+}
+#endif
+
+int64_t MS5607::ms5607_sample_callback(alarm_id_t id, void* user_data) {
+#if ( USE_FREERTOS == 1 )
+ BaseType_t xHigherPriorityTaskWoken;
+ xHigherPriorityTaskWoken = pdFALSE;
+ MS5607* alt = (MS5607 *) user_data;
+ // Defer ISR handling to separate handler within FreeRTOS context
+ vTaskNotifyGiveFromISR(alt->sample_handler_task_handle, &xHigherPriorityTaskWoken );
+ portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+#else
+ MS5607* alt = (MS5607 *) user_data;
+ alt->ms5607_sample();
+#endif
+ return 0;
+}
+
+void MS5607::ms5607_sample() {
+ ms5607_cmd cmd = {.data = 0};
+
+ switch (sample_state) {
+ case NOT_SAMPLING:
+ break;
+ case PRESSURE_CONVERT: {
+ cmd.fields.CONVERT = 1;
+ cmd.fields.TYPE = TYPE_UNCOMPENSATED_PRESSURE;
+ cmd.fields.ADDR_OSR = OSR_CONVERT_256;
+
+ ms5607_write_cmd(&cmd);
+
+ add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true);
+
+ sample_state = TEMPERATURE_CONVERT;
+ break;
+ };
+ case TEMPERATURE_CONVERT: {
+ cmd.data = ADC_READ_COMMAND;
+ ms5607_write_cmd(&cmd);
+ i2c_read_blocking(i2c, addr, buffer, 3, false);
+ uncompensated_pressure = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]);
+
+ cmd.fields.CONVERT = 1;
+ cmd.fields.TYPE = TYPE_UNCOMPENSATED_TEMPERATURE;
+ cmd.fields.ADDR_OSR = OSR_CONVERT_256;
+
+ ms5607_write_cmd(&cmd);
+
+ add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true);
+
+ sample_state = COMPENSATE;
+ break;
+ };
+ case COMPENSATE: {
+ cmd.data = ADC_READ_COMMAND;
+ ms5607_write_cmd(&cmd);
+ i2c_read_blocking(i2c, addr, buffer, 3, false);
+ uncompensated_temperature = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]);
+ ms5607_compensate();
+ altitude = pressure_to_altitude(pressure);
+
+
+ sample_state = NOT_SAMPLING;
+ if (threshold_callback != NULL) {
+ if (positive_crossing) {
+ if (altitude >= threshold_altitude) {
+ add_alarm_in_ms(1, threshold_callback, NULL, true);
+ }
+ } else {
+ if (altitude <= threshold_altitude) {
+ add_alarm_in_ms(1, threshold_callback, NULL, true);
+ }
+ }
+ }
+ break;
+ };
+ };
+
+}
+
+void MS5607::ms5607_compensate() {
+ int32_t dT = uncompensated_temperature - (((uint32_t) prom[4]) << 8);
+ temperature = 2000 + ( ( ( (int64_t) dT) * ( (int64_t) prom[5]) ) >> 23);
+ int64_t OFF = ( ( (int64_t) prom[1]) << 17) + ( ( ((int64_t) prom[3]) * ( (int64_t) dT)) >> 6);
+ int64_t SENS = ( ( (int64_t) prom[0]) << 16) + (( ( (int64_t) prom[2]) * ((int64_t) dT)) >> 7);
+ pressure = (int32_t) ((((((int64_t) uncompensated_pressure) * SENS) >> 21) - OFF) >> 15);
+
+}
+
+int32_t MS5607::pressure_to_altitude(int32_t pressure) {
+ uint16_t o;
+ int16_t part;
+ int32_t low, high;
+
+ if (pressure < 0) {
+ pressure = 0;
+ }
+
+ if (pressure > 120000) {
+ pressure = 120000;
+ }
+
+ o = (uint16_t) (pressure >> ALT_SHIFT);
+ part = pressure & ALT_MASK;
+
+ low = (int32_t) altitude_table[o] * (ALT_SCALE - part);
+ high = (int32_t) altitude_table[o + 1] * part;
+ return ((low + high + (ALT_SCALE >> 1)) >> ALT_SHIFT);
+}
+
+
+void MS5607::set_threshold_altitude(int32_t threshold_altitude, alarm_callback_t callback) {
+ this->threshold_altitude = threshold_altitude;
+
+ positive_crossing = (threshold_altitude > altitude);
+
+ threshold_callback = callback;
+}
+
+void MS5607::clear_threshold_altitude() {
+ this->threshold_callback = NULL;
+}
diff --git a/src/pwm.cpp b/src/pwm.cpp
index 3a78d23..267765b 100644
--- a/src/pwm.cpp
+++ b/src/pwm.cpp
@@ -2,9 +2,9 @@
void PWM::init() {
// Tell GPIO 0 they are allocated to the PWM
- gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
+ gpio_set_function(MICRO_DEFAULT_PWM, GPIO_FUNC_PWM);
// Find out which PWM slice is connected to GPIO 0 (it's slice 0)
- uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
+ uint slice_num = pwm_gpio_to_slice_num(MICRO_DEFAULT_PWM);
// Configure PWM slice and set it running
pwm_config cfg = pwm_get_default_config();
@@ -24,7 +24,7 @@ void PWM::set_duty_cycle(int duty_cycle_percent) {
uint32_t raw_value = WRAP_VALUE * (duty_cycle_percent / 100.0);
// Set the duty cycle
- pwm_set_gpio_level(SERVO_PIN, raw_value);
+ pwm_set_gpio_level(MICRO_DEFAULT_PWM, raw_value);
}
void PWM::set_servo_percent(int percent) {
diff --git a/src/serial.cpp b/src/serial.cpp
new file mode 100644
index 0000000..9dfd5d9
--- /dev/null
+++ b/src/serial.cpp
@@ -0,0 +1,134 @@
+#include "serial.hpp"
+#include "portable.h"
+#include "portmacro.h"
+#include <pico/multicore.h>
+#include <pico/stdio.h>
+
+void serial_task( void *pvParameters ) {
+ char *str = (char *) malloc(1024);
+ size_t len = 1024;
+
+
+ printf("Welcome! :3\n");
+
+ while (1) {
+ printf("# ");
+ stdio_flush();
+
+ size_t n = input_line(str,len);
+ printf("\n");
+ if (n > 0) {
+ for (int i = 0; i < (NUM_BASE_CMDS + num_user_cmds); i++) {
+ if ((i < NUM_BASE_CMDS) && n >= base_commands[i].len && strncmp(str, base_commands[i].name, base_commands[i].len) == 0) {
+ base_commands[i].function();
+ break;
+ }
+
+ if ((i >= NUM_BASE_CMDS && ((i - NUM_BASE_CMDS) < num_user_cmds)) && n >= user_commands[i - NUM_BASE_CMDS].len && strncmp(str, user_commands[i - NUM_BASE_CMDS].name, user_commands[i - NUM_BASE_CMDS].len) == 0) {
+ user_commands[i - NUM_BASE_CMDS].function();
+ break;
+ }
+ if (i == (NUM_BASE_CMDS + num_user_cmds - 1)) {
+ printf("Invalid command! Please try again or use 'help' to see the available commands.\n");
+ }
+ }
+ }
+ }
+}
+
+int input_line(char *buffer, size_t len) {
+ size_t n = 0;
+ int c;
+ for (n = 0; n < len - 1; n++) {
+
+ c = getchar_timeout_us(0);
+ switch (c) {
+ case 127: /* fall through to below */
+ case '\b': /* backspace character received */
+ if (n > 0)
+ n--;
+ buffer[n] = 0;
+ stdio_putchar('\b'); /* output backspace character */
+ stdio_putchar(' ');
+ stdio_putchar('\b');
+ n--; /* set up next iteration to deal with preceding char location */
+ break;
+ case '\n': /* fall through to \r */
+ case '\r':
+ buffer[n] = 0;
+ return n;
+ default:
+ if (c != PICO_ERROR_TIMEOUT && c < 256) {
+ stdio_putchar(c);
+ buffer[n] = c;
+ } else {
+ n--;
+ }
+ break;
+ }
+ }
+ buffer[len - 1] = 0;
+ return 0; // Filled up buffer without reading a linebreak
+}
+
+void info_cmd_func() {
+ extern const char* executeable_name;
+ printf("%s", logo);
+ printf("#####################################################################################\n");
+ printf("# \e[0;31mRocketry \e[0;37mat \e[0;33mVirginia Tech\e[0;37m #\n");
+ printf("# Executeable: %s #\n", executeable_name);
+ printf("#####################################################################################\n\n");
+}
+
+void help_cmd_func() {
+ printf("Commands: \n");
+ for (int i = 0; i < NUM_BASE_CMDS; i++) {
+ printf("\t%s\n", base_commands[i].name);
+ }
+
+ for (int i = 0; i < num_user_cmds; i++) {
+ printf("\t%s\n", user_commands[i].name);
+ }
+}
+
+void clear_cmd_func() {
+ printf("%c%c%c%c",0x1B,0x5B,0x32,0x4A);
+}
+
+void top_cmd_func() {
+#if (configGENERATE_RUN_TIME_STATS == 1)
+ UBaseType_t num_tasks = uxTaskGetNumberOfTasks();
+ char* buffer = (char *) malloc( 40 * num_tasks );
+ vTaskGetRunTimeStats(buffer);
+ printf("%s", buffer);
+ free(buffer);
+#endif
+#if (configSUPPORT_DYNAMIC_ALLOCATION == 1)
+ HeapStats_t heap_stats;
+ vPortGetHeapStats(&heap_stats);
+ printf("\n\tAvailable Heap Space In Bytes:\t%d\nSize Of Largest Free Block In Bytes:\t%d\nNumber Of Successful Allocations:\t%d\n", heap_stats.xAvailableHeapSpaceInBytes, heap_stats.xSizeOfLargestFreeBlockInBytes, heap_stats.xNumberOfSuccessfulAllocations);
+#endif
+}
+
+static void reset_cmd_task(void * unused_arg) {
+ reset_cmd_func();
+}
+
+void reset_cmd_func() {
+#define AIRCR_Register (*((volatile uint32_t*)(PPB_BASE + 0x0ED0C)))
+ if (get_core_num() == 0) {
+ vTaskSuspendAll();
+ multicore_reset_core1();
+ printf("\nOn core zero! Going dark!\n");
+ stdio_flush();
+ AIRCR_Register = 0x5FA0004;
+ } else {
+ printf("\nOn core one! Tasking core zero with the reset!\n");
+ stdio_flush();
+ TaskHandle_t reset_task = NULL;
+ vTaskSuspendAll();
+ xTaskCreate(reset_cmd_task, "reset", 256, NULL, (configMAX_PRIORITIES - 1), &reset_task);
+ vTaskCoreAffinitySet( reset_task, 0x01 );
+ xTaskResumeAll();
+ }
+}
diff --git a/src/spi_flash.c b/src/spi_flash.c
deleted file mode 100644
index 32babc5..0000000
--- a/src/spi_flash.c
+++ /dev/null
@@ -1,267 +0,0 @@
-/**
- * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-// Example of reading/writing an external serial flash using the PL022 SPI interface
-
-#include "spi_flash.h"
-
-static inline void cs_select(uint cs_pin) {
- asm volatile("nop \n nop \n nop"); // FIXME
- gpio_put(cs_pin, 0);
- asm volatile("nop \n nop \n nop"); // FIXME
-}
-
-static inline void cs_deselect(uint cs_pin) {
- asm volatile("nop \n nop \n nop"); // FIXME
- gpio_put(cs_pin, 1);
- asm volatile("nop \n nop \n nop"); // FIXME
-}
-
-void __not_in_flash_func(flash_read)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* dest, size_t len) {
- cs_select(cs_pin);
- uint8_t cmdbuf[4] = {
- FLASH_CMD_READ,
- addr >> 16,
- addr >> 8,
- addr
- };
- spi_write_blocking(spi, cmdbuf, 4);
- spi_read_blocking(spi, 0, dest, len);
- cs_deselect(cs_pin);
-}
-
-void __not_in_flash_func(flash_write_enable)(spi_inst_t *spi, uint cs_pin) {
- cs_select(cs_pin);
- uint8_t cmd = FLASH_CMD_WRITE_EN;
- spi_write_blocking(spi, &cmd, 1);
- cs_deselect(cs_pin);
-}
-
-void __not_in_flash_func(flash_wait_done)(spi_inst_t *spi, uint cs_pin) {
- uint8_t status;
- do {
- cs_select(cs_pin);
- uint8_t buf[2] = {FLASH_CMD_STATUS, 0};
- spi_write_read_blocking(spi, buf, buf, 2);
- cs_deselect(cs_pin);
- status = buf[1];
- } while (status & FLASH_STATUS_BUSY_MASK);
-}
-
-void __not_in_flash_func(flash_sector_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
- uint8_t cmdbuf[4] = {
- FLASH_CMD_SECTOR_ERASE,
- addr >> 16,
- addr >> 8,
- addr
- };
- flash_write_enable(spi, cs_pin);
- cs_select(cs_pin);
- spi_write_blocking(spi, cmdbuf, 4);
- cs_deselect(cs_pin);
- flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_block_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr) {
- uint8_t cmdbuf[4] = {
- FLASH_CMD_BLOCK_ERASE,
- addr >> 16,
- addr >> 8,
- addr
- };
- flash_write_enable(spi, cs_pin);
- cs_select(cs_pin);
- spi_write_blocking(spi, cmdbuf, 4);
- cs_deselect(cs_pin);
- flash_wait_done(spi, cs_pin);
-}
-
-
-void __not_in_flash_func(flash_erase)(spi_inst_t *spi, uint cs_pin) {
- uint8_t cmdbuf[1] = {
- FLASH_CMD_CHIP_ERASE
- };
- flash_write_enable(spi, cs_pin);
- cs_select(cs_pin);
- spi_write_blocking(spi, cmdbuf, 1);
- cs_deselect(cs_pin);
- flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_page_program)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src) {
- uint8_t cmdbuf[4] = {
- FLASH_CMD_PAGE_PROGRAM,
- addr >> 16,
- addr >> 8,
- addr
- };
- flash_write_enable(spi, cs_pin);
- cs_select(cs_pin);
- spi_write_blocking(spi, cmdbuf, 4);
- spi_write_blocking(spi, src, FLASH_PAGE_SIZE);
- cs_deselect(cs_pin);
- flash_wait_done(spi, cs_pin);
-}
-
-void __not_in_flash_func(flash_write)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src, size_t size) {
- uint8_t cmdbuf[4] = {
- FLASH_CMD_PAGE_PROGRAM,
- addr >> 16,
- addr >> 8,
- addr
- };
- flash_write_enable(spi, cs_pin);
- cs_select(cs_pin);
- spi_write_blocking(spi, cmdbuf, 4);
- spi_write_blocking(spi, src, size);
- cs_deselect(cs_pin);
- flash_wait_done(spi, cs_pin);
-}
-
-void write_entry(uint8_t* data_entry) {
- flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
- for (uint16_t i = 0; i < FLASH_PAGE_SIZE; i += PACKET_SIZE) {
- if (page_buffer[i] == 0xFF) {
- base_addr += i;
- break;
- }
- if ((i + PACKET_SIZE) == FLASH_PAGE_SIZE) {
- base_addr += FLASH_PAGE_SIZE;
- flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
- i = 0;
- }
- }
- flash_write(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, data_entry, PACKET_SIZE);
- base_addr += PACKET_SIZE;
-}
-
-
-#ifdef FLASH_TEST
- #include "pico/multicore.h"
- #include <stdint.h>
- #include <inttypes.h>
- #include <stdio.h>
-
-
- void printbuf(uint8_t buf[FLASH_PAGE_SIZE]) {
- for (int i = 0; i < FLASH_PAGE_SIZE; ++i) {
- if (i % 16 == 15)
- printf("%02x\n", buf[i]);
- else
- printf("%02x ", buf[i]);
- }
- }
-
-
-
- void core1_entry() {
- printf("SPI flash example\n");
-
- // Enable SPI 0 at 1 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);
-
- // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN);
- // flash_sector_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0);
- printf("Before program:\n");
- flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
- printbuf(page_buffer);
-
- uint8_t entry[PACKET_SIZE];
-
- printf("Written Data:\n");
- printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\n");
- for (uint16_t i = 0; i < 500; i++) {
- absolute_time_t now = get_absolute_time();
- uint64_t now_us= to_us_since_boot(now);
- float altitude = 10.0f * i;
- float velocity = 5.0f * i;
- uint8_t deploy_percent = (i*1000) / 200;
- printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\tDAWSYN_SCHRAIB\n", now_us, (uint8_t)(i), deploy_percent, altitude, velocity);
- uint32_t alt_bits = *((uint32_t *)&altitude);
- uint32_t vel_bits = *((uint32_t *)&velocity);
- entry[0] = now_us >> 56;
- entry[1] = now_us >> 48;
- entry[2] = now_us >> 40;
- entry[3] = now_us >> 32;
- entry[4] = now_us >> 24;
- entry[5] = now_us >> 16;
- entry[6] = now_us >> 8;
- entry[7] = now_us;
- entry[8] = i;
- entry[9] = deploy_percent;
- entry[10] = alt_bits >> 24;
- entry[11] = alt_bits >> 16;
- entry[12] = alt_bits >> 8;
- entry[13] = alt_bits;
- entry[14] = vel_bits >> 24;
- entry[15] = vel_bits >> 16;
- entry[16] = vel_bits >> 8;
- entry[17] = vel_bits;
- entry[18] = 'D';
- entry[19] = 'A';
- entry[20] = 'W';
- entry[21] = 'S';
- entry[22] = 'Y';
- entry[23] = 'N';
- entry[24] = '_';
- entry[25] = 'S';
- entry[26] = 'C';
- entry[27] = 'H';
- entry[28] = 'R';
- entry[29] = 'A';
- entry[30] = 'I';
- entry[31] = 'B';
- write_entry(entry);
- }
-
- printf("After program:\n");
- flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, 0, page_buffer, FLASH_PAGE_SIZE);
- printbuf(page_buffer);
-
- printf("\nRead Data:\n");
- printf("time (us)\t|\tstate\t|\tdep pcnt\t|\talt (m)\t|\tvel (m/s)\t|\tempty\n");
- for (uint32_t i = 0; i < base_addr; i += PACKET_SIZE) {
- flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, i, entry, PACKET_SIZE);
- uint64_t now_us = (((uint64_t)entry[0] << 56) | ((uint64_t)entry[1] << 48) | \
- ((uint64_t)entry[2] << 40) | ((uint64_t)entry[3] << 32) | \
- ((uint64_t)entry[4] << 24) | ((uint64_t)entry[5] << 16) | \
- ((uint64_t)entry[6] << 8) | ((uint64_t)entry[7]));
-
- uint8_t state = entry[8];
- uint8_t deploy_percent = entry[9];
-
- uint32_t alt_bits = (entry[10] << 24) | (entry[11] << 16) | (entry[12] << 8) | (entry[13]);
- uint32_t vel_bits = (entry[14] << 24) | (entry[15] << 16) | (entry[16] << 8) | (entry[17]);
- float altitude = *(float *)(&alt_bits);
- float velocity = *(float *)(&vel_bits);
- printf("%"PRIu64"\t|\t%"PRIu8"\t|\t%"PRIu8"\t|\t%4.2f\t|\t%4.2f\t|\t%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n", \
- now_us, state, deploy_percent, altitude, velocity, \
- entry[18],entry[19],entry[20],entry[21],entry[22],entry[23],entry[24],entry[25],entry[26],entry[27],entry[28],entry[29],entry[30],entry[31]);
- }
-
- }
-
- int main() {
- // Enable UART so we can print status output
- stdio_init_all();
- getchar();
-
- multicore_launch_core1(core1_entry);
-
- while (1) {
- tight_loop_contents();
- }
-
- }
-#endif
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-12 17:25:23 +0000