diff options
Diffstat (limited to 'tools/read_flash.cpp')
| -rw-r--r-- | tools/read_flash.cpp | 538 |
1 files changed, 538 insertions, 0 deletions
diff --git a/tools/read_flash.cpp b/tools/read_flash.cpp new file mode 100644 index 0000000..b3305aa --- /dev/null +++ b/tools/read_flash.cpp @@ -0,0 +1,538 @@ +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <inttypes.h> + +#include <cmath> //TODO: Reimplement math properly using Eigen functions +#include <sstream> //TODO: This is for debug purposes, remove when implemented +#include <Eigen/Dense> +using namespace Eigen; //TODO: Limit scope to necessary components once implemented + +#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/error.h> +#include <pico/types.h> + +/* Kernel includes. */ +#include "FreeRTOS.h" +#include "FreeRTOSConfig.h" +#include "portmacro.h" +#include "projdefs.h" +#include "serial.hpp" +#include "task.h" +#include "semphr.h" + +#include "heartbeat.hpp" +#include "adxl375.hpp" +#include "ms5607.hpp" +#include "iim42653.hpp" +#include "mmc5983ma.hpp" +#include "serial.hpp" +#include "pico_logger.h" +#include "log_format.hpp" + +/* Priorities at which the tasks are created. */ +#define EVENT_HANDLER_PRIORITY ( tskIDLE_PRIORITY + 4 ) +#define SENSOR_SAMPLE_PRIORITY ( tskIDLE_PRIORITY + 3 ) +#define HEARTBEAT_TASK_PRIORITY ( tskIDLE_PRIORITY + 2 ) +#define LOGGING_PRIORITY ( tskIDLE_PRIORITY + 2 ) +#define SERIAL_TASK_PRIORITY ( tskIDLE_PRIORITY + 1 ) + +#define SENSOR_SAMPLE_RATE_HZ 500 +#define ORIENTATION_ESTIMATION_RATE_HZ 5 + +#define MAX_SCL 400000 + +static void populate_log_entry(log_entry_t * log_entry); +static void sample_cmd_func(); +static void debug_cmd_func(); +static void circular_cmd_func(); +static void read_cmd_func(); +static void write_cmd_func(); +static void erase_cmd_func(); +static void orient_cmd_func(); +static void logging_task(void * unused_arg); + +void vApplicationTickHook(void) { /* optional */ } +void vApplicationMallocFailedHook(void) { /* optional */ } +void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { for( ;; ); } + +const char* executeable_name = "read_flash.uf2"; +const size_t num_user_cmds = 7; +const command_t user_commands[] = { {.name = "sample", + .len = 6, + .function = &sample_cmd_func}, + {.name = "debug", + .len = 5, + .function = &debug_cmd_func}, + {.name = "circular", + .len = 8, + .function = &circular_cmd_func}, + {.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 = "orient", + .len = 6, + .function = &orient_cmd_func} }; + +volatile bool serial_data_output = false; +volatile bool use_circular_buffer = false; + +MS5607 alt(i2c_default); +ADXL375 adxl375(i2c_default); +IIM42653 iim42653(i2c_default); +MMC5983MA mmc5983ma(i2c_default); + +Logger logger(PACKET_SIZE, LOG_BASE_ADDR, print_log_entry); + +volatile TaskHandle_t logging_handle = NULL; + +int main() { + stdio_init_all(); + + adc_init(); + adc_set_temp_sensor_enabled(true); + + gpio_init(PICO_DEFAULT_LED_PIN); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + gpio_put(PICO_DEFAULT_LED_PIN, 0); + + i2c_init(i2c_default, MAX_SCL); + 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); + + sleep_ms(2500); + + info_cmd_func(); + stdio_flush(); + + logger.initialize(true); + + alt.initialize(); + sleep_ms(500); + adxl375.initialize(); + sleep_ms(500); + iim42653.initialize(); + sleep_ms(500); + mmc5983ma.initialize(); + sleep_ms(500); + + xTaskCreate(heartbeat_task, "heartbeat", 256, NULL, HEARTBEAT_TASK_PRIORITY, NULL); + xTaskCreate(serial_task, "serial", 8192, NULL, SERIAL_TASK_PRIORITY, NULL); + + vTaskStartScheduler(); + + while (1) { + tight_loop_contents(); + } +} + +static void logging_task(void * unused_arg) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / 10); + + xLastWakeTime = xTaskGetTickCount(); + printf("Time,Pressure,Altitude,Temperature,ax,ay,az,ax,ay,az,gx,gy,gz,ax,ay,az\n"); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); +// printf("%" PRIu64 ",", time_us_64()); +// printf("%4.2f,", ((float) alt.get_pressure()) / PRESSURE_SCALE_F); +// printf("%4.2f,", ((float) alt.get_altitude()) / ALTITUDE_SCALE_F); +// printf("%4.2f,", ((float) alt.get_temperature()) / TEMPERATURE_SCALE_F); +// printf("%4.2f,", adxl375.scale(adxl375.get_ax())); +// printf("%4.2f,", adxl375.scale(adxl375.get_ay())); +// printf("%4.2f,", adxl375.scale(adxl375.get_az())); +// printf("%4.2f,", iim42653.scale_accel(iim42653.get_ax())); +// printf("%4.2f,", iim42653.scale_accel(iim42653.get_ay())); +// printf("%4.2f,", iim42653.scale_accel(iim42653.get_az())); +// printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gx())); +// printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gy())); +// printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gz())); +// printf("%" PRIi16 ",", mmc5983ma.get_ax()); +// printf("%" PRIi16 ",", mmc5983ma.get_ay()); +// printf("%" PRIi16 ",", mmc5983ma.get_az()); +// printf("\r\n"); + 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 { + logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true); + } + stdio_flush(); + } + +} + +//TODO: This task is getting increasingly complex and involved, it may be worth migrating to a class of its own +static void pose_estimation_task(void * unused_arg) { + printf("--POSE: INITIALIZING DATA MEMBERS\n"); + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ORIENTATION_ESTIMATION_RATE_HZ); + xLastWakeTime = xTaskGetTickCount(); + int orient_count = 0; + + Quaternionf q_k(1, 0, 0, 0); //Initialize to straight upright + Quaternionf q_k_est = q_k; + Matrix4f P_k = Matrix4f::Identity()*0.01f; //TODO: Tune this initialization value + Matrix4f P_k_est = P_k; + + //TODO: Store covariance values somewhere reasonable instead of hardcoding them + Matrix3f gyro_covariance = Matrix3f::Identity()*0.05f*M_PI/180.0f; //0.05deg/s RMSE @ 100HZ Bandwidth + Matrix<float, 6, 6> accel_mag_covariance; + accel_mag_covariance << 0.00065f, 0, 0, 0, 0, 0, + 0, 0.00065f, 0, 0, 0, 0, + 0, 0, 0.00070f, 0, 0, 0, + 0, 0, 0, 0.00120f, 0, 0, + 0, 0, 0, 0, 0.00120f, 0, + 0, 0, 0, 0, 0, 0.00120f; + //accel_mag_covariance *= 3; + + //Stored intermediate equation matrices + Matrix4f gyro_skew; //Skew-symmetric matrix equivalent of gyroscope output + Matrix4f I_4 = Matrix4f::Identity(); //4x4 Identity matrix, used for F_k calculation + + Matrix4f F_k; //State Transition Matrix + Matrix4f H_k; //Observation Matrix + Matrix4f K_k; //Kalman Gain Matrix + Matrix<float, 4, 3> J_process_k; //Jacobian for process (gyro) covariance transformation + Matrix<float, 4, 6> J_observation_k; //Jacobian for observation (accel/mag) covariance transformation + Matrix4f process_noise_transformed; //Matrix of transformed process noise + Matrix4f observation_noise_transformed; //Matrix of transformed observation noise + Matrix<float, 4, 1> z_k; //Measurement vector + Matrix<float, 4, 1> z_k_est; //Estimated measurement vector, used for Kalman filter calculation + + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + + //printf("-------- BEGINNING UPDATE COMPUTATION --------\n"); + //Get all required sensor values and compute intermediate values + float imu_ax = -IIM42653::scale_accel(iim42653.get_ax()); + float imu_ay = -IIM42653::scale_accel(iim42653.get_ay()); + float imu_az = IIM42653::scale_accel(iim42653.get_az()); + float imu_gx = -IIM42653::scale_gyro(iim42653.get_gx())/180.0f*M_PI; + float imu_gy = -IIM42653::scale_gyro(iim42653.get_gy())/180.0f*M_PI; + float imu_gz = IIM42653::scale_gyro(iim42653.get_gz())/180.0f*M_PI; + float mag_x = -MMC5983MA::scale_mag(mmc5983ma.get_ay()); //TODO: Not convinced mag axes are correct + float mag_y = MMC5983MA::scale_mag(mmc5983ma.get_ax()); + float mag_z = -MMC5983MA::scale_mag(mmc5983ma.get_az()); + + //Normalize accelerometer and magnetometer measurements + float imu_a_mag = std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_ay, 2) + std::pow(imu_az, 2)); + float mag_mag = std::sqrt(std::pow(mag_x, 2) + std::pow(mag_y, 2) + std::pow(mag_z, 2)); + imu_ax /= imu_a_mag; + imu_ay /= imu_a_mag; + imu_az /= imu_a_mag; + mag_x /= mag_mag; + mag_y /= mag_mag; + mag_z /= mag_mag; + + printf("--- [I] INITIALIZATION | IMU Accel direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_ax, imu_ay, imu_az); + printf("--- [I] INITIALIZATION | IMU Gyro direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_gx, imu_gy, imu_gz); + printf("--- [I] INITIALIZATION | Magnetometer direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", mag_x, mag_y, mag_z); + + float mag_D = imu_ax*mag_x + imu_ay*mag_y + imu_az*mag_z; + float mag_N = std::sqrt(1.0f - std::pow(mag_D, 2)); + printf("--- [I] INITIALIZATION | Magnetometer values (mag_mag, mag_D, mag_N): [%2.4f, %2.4f, %2.4f]\n", mag_mag, mag_D, mag_N); + + //UPDATE | Calculate Kalman gain + //TODO: This Jacobian calculation is FUGLY, and very computationally intensive + // Separate out common math, vectorize, and put it in a function somewhere? + float pd_mD_ax = mag_x/(2*mag_D); + float pd_mD_ay = mag_y/(2*mag_D); + float pd_mD_az = mag_z/(2*mag_D); + float pd_mD_mx = imu_ax/(2*mag_D); + float pd_mD_my = imu_ay/(2*mag_D); + float pd_mD_mz = imu_az/(2*mag_D); + float pd_mN_ax = -mag_D/mag_N*pd_mD_ax; + float pd_mN_ay = -mag_D/mag_N*pd_mD_ay; + float pd_mN_az = -mag_D/mag_N*pd_mD_az; + float pd_mN_mx = -mag_D/mag_N*pd_mD_mx; + float pd_mN_my = -mag_D/mag_N*pd_mD_my; + float pd_mN_mz = -mag_D/mag_N*pd_mD_mz; + float common_num = (imu_ay*mag_z - imu_az*mag_y); + float common_denom = std::pow(mag_N, 2); + + float J_o_4_1 = (-pd_mN_ax*common_num)/common_denom; + float J_o_4_2 = (mag_z*mag_N - pd_mN_ay*common_num)/common_denom; + float J_o_4_3 = (-mag_y*mag_N - pd_mN_az*common_num)/common_denom; + float J_o_4_4 = (-pd_mN_mx*common_num)/common_denom; + float J_o_4_5 = (-imu_az*mag_N - pd_mN_my*common_num)/common_denom; + float J_o_4_6 = (imu_ay*mag_N - pd_mN_mz*common_num)/common_denom; + J_observation_k << 1.0f, 0, 0, 0, 0, 0, + 0, 1.0f, 0, 0, 0, 0, + 0, 0, 1.0f, 0, 0, 0, + J_o_4_1, J_o_4_2, J_o_4_3, J_o_4_4, J_o_4_5, J_o_4_6; + //printf("- [U] CALC KALMAN GAIN | [J_o_4_1 - J_o_4_6] after calculation, from J_observation_k: [%4.2f | %4.2f | %4.2f | %4.2f | %4.2f | %4.2f]\n", J_observation_k(3, 0), J_observation_k(3, 1), J_observation_k(3, 2), J_observation_k(3, 3), J_observation_k(3, 4), J_observation_k(3, 5)); + + observation_noise_transformed = J_observation_k*accel_mag_covariance*J_observation_k.transpose(); + //printf("- [U] CALC KALMAN GAIN | Diagonal of observation_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", observation_noise_transformed(0, 0), observation_noise_transformed(1, 1), observation_noise_transformed(2, 2), observation_noise_transformed(3, 3)); + + H_k << -q_k.y(), q_k.z(), -q_k.w(), q_k.x(), + q_k.x(), q_k.w(), q_k.z(), q_k.y(), + q_k.w(), -q_k.x(), -q_k.y(), q_k.z(), + q_k.z(), q_k.y(), q_k.x(), q_k.w(); + H_k *= 2; + //printf("- [U] CALC KALMAN GAIN | First column of H_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", H_k(0, 0), H_k(1, 0), H_k(2, 0), H_k(3, 0)); + + K_k = P_k*H_k.transpose()*(H_k*P_k*H_k.transpose() + observation_noise_transformed).inverse(); + //printf("- [U] CALC KALMAN GAIN | First column of K_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", K_k(0, 0), K_k(1, 0), K_k(2, 0), K_k(3, 0)); + + //UPDATE | Calculate current state estimate based on accel/mag measurements + z_k << imu_ax, imu_ay, imu_az, ((imu_ay*mag_z - imu_az*mag_y)/mag_N); + z_k_est = H_k*q_k_est.coeffs(); //TODO: Confirm quaternion multiplication, output and check + q_k = q_k_est.coeffs() + K_k*(z_k - z_k_est); //TODO: Confirm quaternion arithmetic, output and check + q_k.normalize(); //Ensure state output is a proper rotation quaternion + //printf("- [U] CALC STATE ESTIMATE | See below block for direct output\n"); + + //UPDATE | Calculate state covariance based on accel/mag measurements + P_k = (I_4 - K_k*H_k)*P_k_est; + //printf("- [U] CALC STATE COVAR | First column of P_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k(0, 0), P_k(1, 1), P_k(2, 2), P_k(3, 3)); + + //printf("-------- COMPLETED UPDATE COMPUTATION! --------\n\n"); + //if (orient_count == ORIENTATION_ESTIMATION_RATE_HZ) { + printf("~~> Estimate (q_k) Coefficients [w, x, y, z]: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k.w(), q_k.x(), q_k.y(), q_k.z()); + Vector3f testVect = q_k.toRotationMatrix().eulerAngles(2, 1, 0)*180.0f/M_PI; + printf("~~> Estimate (q_k) in Euler [Yaw, Pitch, Roll]: [%3.3f, %3.3f, %3.3f]\n", testVect[0], testVect[1], testVect[2]); + Vector3f testAccelVect(imu_ax, imu_ay, imu_az); + Vector3f testAccelVectRotated = q_k._transformVector(testAccelVect); + printf("~~> Estimate (q_k) rotated accel magnitude vector: [%2.4f, %2.4f, %2.4f]\n\n", testAccelVectRotated[0], testAccelVectRotated[1], testAccelVectRotated[2]); + + // orient_count = 0; + //} + //orient_count++; + + //printf("-------- BEGINNING PREDICT COMPUTATION --------\n"); + + //Predict next state based on gyroscope measurements + gyro_skew << 0, -imu_gx, -imu_gy, -imu_gz, + imu_gx, 0, imu_gz, -imu_gy, + imu_gy, -imu_gz, 0, imu_gx, + imu_gz, imu_gy, -imu_gx, 0; + //printf("- [P] PREDICT STATE | Top second element of gyro_skew after set: [%4.2f]\n", gyro_skew(0, 1)); + + F_k = I_4 + (0.5f/ORIENTATION_ESTIMATION_RATE_HZ)*gyro_skew; + //printf("- [P] PREDICT STATE | First column of F_k after set: [%2.4f, %2.4f, %2.4f, %2.4f]\n", F_k(0, 0), F_k(1, 0), F_k(2, 0), F_k(3, 0)); + + q_k_est = F_k*q_k.coeffs(); + q_k_est.normalize(); //TODO: CONFIRM normalization necessary at intermediary steps + //printf("- [P] PREDICT STATE | State estimate (q_k_est) coefficients after predict step: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k_est.w(), q_k_est.x(), q_k_est.y(), q_k_est.z()); + + //Predict next covariance based on gyroscope measurements + Jacobian-transformed measurement variance + J_process_k << q_k.x(), q_k.y(), q_k.z(), + -1.0f*q_k.w(), q_k.z(), -1.0f*q_k.y(), + -1.0f*q_k.z(), -1.0f*q_k.w(), q_k.x(), + q_k.y(), -1.0f*q_k.x(), -1.0f*q_k.w(); + //printf("- [P] PREDICT COVAR | Top left element of J_process_k after computation: [%4.2f]\n", J_process_k(0, 0)); + + process_noise_transformed = std::pow(0.5f/ORIENTATION_ESTIMATION_RATE_HZ, 2) * + J_process_k*gyro_covariance*J_process_k.transpose(); + //printf("- [P] PREDICT COVAR | Diagonal of process_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", process_noise_transformed(0, 0), process_noise_transformed(1, 1), process_noise_transformed(2, 2), process_noise_transformed(3, 3)); + + P_k_est = F_k*P_k*F_k.transpose() + process_noise_transformed; + //printf("- [P] PREDICT COVAR | Diagonal of P_k_est after prediction computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k_est(0, 0), P_k_est(1, 1), P_k_est(2, 2), P_k_est(3, 3)); + //printf("-------- PREDICT COMPUTATION COMPLETED! --------\n\n\n"); + } +} + +static void sample_cmd_func() { + static bool sampling = false; + + if (sampling == false) { + vTaskSuspendAll(); + + 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)); + + xTaskCreate(MS5607::ms5607_sample_handler, "ms5607_sample_handler", 256, &alt, EVENT_HANDLER_PRIORITY, &(alt.sample_handler_task_handle)); + + vTaskCoreAffinitySet( alt.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( adxl375.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( iim42653.update_task_handle, 0x01 ); + vTaskCoreAffinitySet( mmc5983ma.update_task_handle, 0x01 ); + + vTaskCoreAffinitySet( alt.sample_handler_task_handle, 0x01 ); + sampling = true; + xTaskResumeAll(); + } else { + printf("Stopping sample!\n"); + vTaskSuspendAll(); + + vTaskDelete(alt.update_task_handle); + vTaskDelete(adxl375.update_task_handle); + vTaskDelete(iim42653.update_task_handle); + vTaskDelete(mmc5983ma.update_task_handle); + + vTaskDelete(alt.sample_handler_task_handle); + + alt.update_task_handle = NULL; + adxl375.update_task_handle = NULL; + iim42653.update_task_handle = NULL; + mmc5983ma.update_task_handle = NULL; + + alt.sample_handler_task_handle = NULL; + + sampling = false; + xTaskResumeAll(); + } +} + + +static void debug_cmd_func() { + if (logging_handle == NULL) { + vTaskSuspendAll(); + xTaskCreate(logging_task, "logging", 256, NULL, LOGGING_PRIORITY, const_cast<TaskHandle_t *>(&logging_handle)); + vTaskCoreAffinitySet(logging_handle, 0x02); + xTaskResumeAll(); + } else { + vTaskSuspendAll(); + vTaskDelete(logging_handle); + logging_handle = NULL; + xTaskResumeAll(); + } +} + +static void circular_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + if (!use_circular_buffer) { + logger.initialize_circular_buffer(PAD_BUFFER_SIZE); + use_circular_buffer = true; + } else { + logger.flush_circular_buffer(true); + use_circular_buffer = false; + } + if (logging_handle != NULL) { + vTaskResume(logging_handle); + } +} + +static void read_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + if (use_circular_buffer) { + logger.read_circular_buffer(); + } else { + logger.read_memory(); + } + vTaskResume(logging_handle); +} + +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 = PAD; + log_entry->deploy_percent = 80; + log_entry->pressure = alt.get_pressure(); + log_entry->altitude = alt.get_altitude(); + 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->data0 = get_rand_64(); + log_entry->data1 = get_rand_64(); + log_entry->data2 = get_rand_32(); + log_entry->data3 = get_rand_32(); +} + +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 { + 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); + } +} + +static void erase_cmd_func() { + if (logging_handle != NULL) { + vTaskSuspend(logging_handle); + } + logger.erase_memory(); + if (logging_handle != NULL) { + vTaskResume(logging_handle); + } +} + +static void orient_cmd_func() { + /* + float pitch_rad = std::atan(imu_ay/std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_az, 2))); + float roll_rad = std::atan(imu_ax/std::sqrt(std::pow(imu_ay, 2) + std::pow(imu_az, 2))); + float yaw_rad = std::atan2(-mag_y*std::cos(roll_rad) + mag_z*std::sin(roll_rad), + mag_x*std::cos(pitch_rad) + mag_y*std::sin(pitch_rad)*std::sin(roll_rad) + mag_z*std::cos(roll_rad) * std::sin(pitch_rad)); + */ + + static TaskHandle_t pose_estimation_handle = NULL; + static bool estimating = false; + + if (!estimating) { + printf("================ BEGINNING STATE ESTIMATION ================\n"); + vTaskSuspendAll(); + + //TODO: Assign pose estimation unique priority? + xTaskCreate(pose_estimation_task, "pose_estimation", 1024, NULL, + SENSOR_SAMPLE_PRIORITY, &pose_estimation_handle); + vTaskCoreAffinitySet( pose_estimation_handle, 0x01 ); + + estimating = true; + xTaskResumeAll(); + } else { + printf("================= ENDING STATE ESTIMATION ================\n"); + vTaskSuspendAll(); + + vTaskDelete(pose_estimation_handle); + pose_estimation_handle = NULL; + + estimating = false; + xTaskResumeAll(); + } +} + |