diff options
Diffstat (limited to 'tools')
| -rw-r--r-- | tools/CMakeLists.txt | 42 | ||||
| -rw-r--r-- | tools/alt_test.cpp | 51 | ||||
| -rw-r--r-- | tools/clock_gen_test.cpp | 22 | ||||
| -rw-r--r-- | tools/imu_calib.cpp | 222 | ||||
| -rw-r--r-- | tools/read_flash.c | 78 | ||||
| -rw-r--r-- | tools/read_flash.cpp | 538 | ||||
| -rw-r--r-- | tools/servo_test.cpp | 8 |
7 files changed, 610 insertions, 351 deletions
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 8806fb0..a5a7ba6 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -1,34 +1,42 @@ if (COMPILE_TOOLS) add_executable(read_flash - read_flash.c - ../src/spi_flash.c + read_flash.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/log_format.cpp + ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp + ${PROJECT_SOURCE_DIR}/src/serial.cpp ) add_executable(servo_test servo_test.cpp - ../src/pwm.cpp + ${PROJECT_SOURCE_DIR}/src/pwm.cpp ) add_executable(alt_test alt_test.cpp - ) - - add_executable(imu_calib - imu_calib.cpp + ${PROJECT_SOURCE_DIR}/src/ms5607.cpp + ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp ) # pull in common dependencies - target_link_libraries(read_flash pico_stdlib hardware_spi) - target_include_directories(read_flash PUBLIC ../include) + target_link_libraries(read_flash pico_stdlib pico_logger pico_flash pico_rand pico_multicore pico_sync hardware_i2c hardware_adc hardware_timer FreeRTOS-Kernel FreeRTOS-Kernel-Heap4) + target_include_directories(read_flash PUBLIC ${PROJECT_SOURCE_DIR}/include) + target_compile_definitions(read_flash PRIVATE + USE_FREERTOS=1 + DEBUG=1 + PICO_STDIO_STACK_BUFFER_SIZE=64 # use a small printf on stack buffer + ) + pico_set_binary_type(read_flash copy_to_ram) target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c) - target_include_directories(servo_test PUBLIC ../include) + target_include_directories(servo_test PUBLIC ${PROJECT_SOURCE_DIR}/include) target_link_libraries(alt_test pico_stdlib hardware_i2c hardware_gpio) - - target_link_libraries(imu_calib pico_stdlib hardware_i2c hardware_gpio) - target_include_directories(imu_calib PUBLIC ../include) - + target_include_directories(alt_test PUBLIC ${PROJECT_SOURCE_DIR}/include) + pico_enable_stdio_usb(read_flash 1) pico_enable_stdio_uart(read_flash 0) @@ -37,14 +45,10 @@ if (COMPILE_TOOLS) pico_enable_stdio_usb(alt_test 1) pico_enable_stdio_uart(alt_test 0) - - pico_enable_stdio_usb(imu_calib 1) - pico_enable_stdio_uart(imu_calib 0) - + # create map/bin/hex file etc. pico_add_extra_outputs(read_flash) pico_add_extra_outputs(servo_test) pico_add_extra_outputs(alt_test) - pico_add_extra_outputs(imu_calib) endif() diff --git a/tools/alt_test.cpp b/tools/alt_test.cpp index a5c8849..9e92cdd 100644 --- a/tools/alt_test.cpp +++ b/tools/alt_test.cpp @@ -1,50 +1,47 @@ #include <stdio.h> #include "hardware/gpio.h" -#include "boards/pico_w.h" #include "hardware/i2c.h" #include "pico/stdio.h" #include "pico/time.h" -#define ALT_ADDR 0x60 +#include "ms5607.hpp" +#include "heartbeat.hpp" +#include "rp2040_micro.h" + +#define DATA_RATE_HZ 200 #define MAX_SCL 400000 -#define DATA_RATE_HZ 15 -float altitude = 0.0f; -float get_altitude(); +bool data_callback(repeating_timer_t *rt); + +repeating_timer_t data_timer; + +MS5607 altimeter(i2c_default); int main() { stdio_init_all(); i2c_init(i2c_default, MAX_SCL); - gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_init(PICO_DEFAULT_I2C_SCL_PIN); + gpio_init(PICO_DEFAULT_I2C_SDA_PIN); 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); + gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C); - uint8_t config[2] = {0}; + altimeter.initialize(); + heartbeat_initialize(PICO_DEFAULT_LED_PIN); + // gpio_init(PICO_DEFAULT_LED_PIN); + // gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + // gpio_put(PICO_DEFAULT_LED_PIN, 1); - // Select control register(0x26) - // Active mode, OSR = 16, altimeter mode(0xB8) - config[0] = 0x26; - config[1] = 0xB9; - i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); - sleep_ms(1500); + add_repeating_timer_us(-1000000 / DATA_RATE_HZ, &data_callback, NULL, &data_timer); while (1) { - sleep_ms(1000); - altitude = get_altitude(); - printf("Altitude: %4.2f\n", altitude); + sleep_ms(500); + printf("\n\nAltitude: %4.2f\nPressure: %4.2f\nTemperature: %4.2f\n", (float) altimeter.get_altitude() / ALTITUDE_SCALE_F, (float) altimeter.get_pressure() / PRESSURE_SCALE_F, (float) altimeter.get_temperature() / TEMPERATURE_SCALE_F); } } -float get_altitude() { - uint8_t reg = 0x01; - uint8_t data[5]; - i2c_write_blocking(i2c_default, ALT_ADDR, ®, 1, true); - i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false); - // Exactly how MPL3115A2 datasheet says to retrieve altitude - float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625; - return altitude; +bool data_callback(repeating_timer_t *rt) { + altimeter.ms5607_start_sample(); + return true; } - diff --git a/tools/clock_gen_test.cpp b/tools/clock_gen_test.cpp new file mode 100644 index 0000000..6cfdcdd --- /dev/null +++ b/tools/clock_gen_test.cpp @@ -0,0 +1,22 @@ +#include <stdio.h> + +#include "boards/pico_w.h" +#include "pico/stdio.h" +#include "hardware/gpio.h" +#include "hardware/clocks.h" +#include "pico/time.h" + +//Generate a 40kHz square wave with duty 0.5 on pin 21 +int main() { + stdio_init_all(); + + //21 --> GPIO Selection, Pin 21 (Clock 0) + //0x6 --> Input selection, CLK_SYS as input (125MHz) + //3125 --> Clock Divider Int, 125MHz/40kHz = 3125 + clock_gpio_init(21, 0x6, 3125); + + while(1) { + printf("Clock may or may not be running?"); + sleep_ms(1000); + } +} diff --git a/tools/imu_calib.cpp b/tools/imu_calib.cpp deleted file mode 100644 index dc45f39..0000000 --- a/tools/imu_calib.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include <stdio.h> -#include <stdint.h> -#include <inttypes.h> -#include <Eigen/Geometry> - -#include "pico/stdio.h" -#include "hardware/gpio.h" -#include "hardware/i2c.h" - -#define MAX_SCL 400000 - -#define BNO055_OPR_MODE_ADDR 0x3D -#define BNO055_OPR_MODE_CONFIG 0x00 -#define BNO055_SYS_TRIGGER_ADDR 0x3F -#define BNO055_ADDRESS 0x28 -#define BNO055_CHIP_ID_ADDR 0x00 -#define BNO055_CHIP_ID 0xA0 -#define BNO055_OPR_MODE_NDOF 0x0C -#define BNO055_CALIB_STAT_ADDR 0x35 -#define ACCEL_OFFSET_X_LSB_ADDR 0x55 -#define BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR 0x28 -#define BNO055_QUATERNION_DATA_W_LSB_ADDR 0x20 -#define UNIT_SELECTION 0x3B - -void get_calibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag); - -int main() { - stdio_init_all(); - - getchar(); - - 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); - - uint8_t buf[2] = {BNO055_CHIP_ID_ADDR}; - - uint8_t id = 0x00; - sleep_ms(1000); - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false); - while (id != BNO055_CHIP_ID) { - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false); - printf("Id not correct!, seeing: %" PRIu8 "\n", id); - sleep_ms(10); - } - - buf[0] = BNO055_OPR_MODE_ADDR; - buf[1] = BNO055_OPR_MODE_CONFIG; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - - buf[0] = BNO055_SYS_TRIGGER_ADDR; - buf[1] = 0x20; // RESET - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - sleep_ms(30); - - buf[0] = BNO055_CHIP_ID_ADDR; - id = 0x00; - while (id != BNO055_CHIP_ID) { - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false); - printf("Id not correct!, seeing: %" PRIu8 "\n", id); - sleep_ms(10); - } - - buf[0] = BNO055_SYS_TRIGGER_ADDR; - buf[1] = 0x00; // RESET - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - sleep_ms(30); - - // Set units to m/s^2 - buf[0] = UNIT_SELECTION; - buf[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2 - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - sleep_ms(50); - - buf[0] = BNO055_OPR_MODE_ADDR; - buf[1] = BNO055_OPR_MODE_NDOF; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - - uint8_t gyro = 0x00, accel = 0x00, mag = 0x00; - - printf("Magnetometer: Perform the figure-eight calibration dance.\n"); - while (mag != 3) { - // Calibration Dance Step One: Magnetometer - // Move sensor away from magnetic interference or shields - // Perform the figure-eight until calibrated - get_calibration(NULL, NULL, NULL, &mag); - printf("Mag Calib Status: %3.0f\n", (100 / 3 * mag)); - sleep_ms(1000); - } - printf("... CALIBRATED\n"); - sleep_ms(1000); - - printf("Accelerometer: Perform the six-step calibration dance.\n"); - while (accel != 3) { - // Calibration Dance Step Two: Accelerometer - // Place sensor board into six stable positions for a few seconds each: - // 1) x-axis right, y-axis up, z-axis away - // 2) x-axis up, y-axis left, z-axis away - // 3) x-axis left, y-axis down, z-axis away - // 4) x-axis down, y-axis right, z-axis away - // 5) x-axis left, y-axis right, z-axis up - // 6) x-axis right, y-axis left, z-axis down - // Repeat the steps until calibrated - get_calibration(NULL, NULL, &accel, NULL); - printf("Accel Calib Status: %3.0f\n", (100 / 3 * accel)); - sleep_ms(1000); - } - printf("... CALIBRATED\n"); - sleep_ms(1000); - - printf("Gyroscope: Perform the hold-in-place calibration dance.\n"); - while (gyro != 3) { - // Calibration Dance Step Three: Gyroscope - // Place sensor in any stable position for a few seconds - // (Accelerometer calibration may also calibrate the gyro) - get_calibration(NULL, &gyro, NULL, NULL); - printf("Gyro Calib Status: %3.0f\n", (100 / 3 * gyro)); - sleep_ms(1000); - } - printf("... CALIBRATED\n"); - sleep_ms(1000); - printf("CALIBRATION COMPLETED\n"); - - // Get Sensor Offsets - buf[0] = BNO055_OPR_MODE_ADDR; - buf[1] = BNO055_OPR_MODE_CONFIG; - uint8_t sensor_offsets[22]; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - sleep_ms(30); - - buf[0] = ACCEL_OFFSET_X_LSB_ADDR; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, sensor_offsets, 18, false); - for (uint8_t i = 0; i < 18; i++) { - printf("sensor_offsets[%" PRIu8 "] = 0x%" PRIx8 ";\r\n", i + 1, sensor_offsets[i]); - } - sleep_ms(5000); - - buf[0] = BNO055_OPR_MODE_ADDR; - buf[1] = BNO055_OPR_MODE_NDOF; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false); - sleep_ms(5000); - - getchar(); - - uint8_t lin_accel[6]; - uint8_t quat[8]; - float accel_x, accel_y, accel_z; - float abs_lin_accel_x, abs_lin_accel_y, abs_lin_accel_z; - float abs_quaternion_w, abs_quaternion_x, abs_quaternion_y, abs_quaternion_z; - while (1) { - uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, &lin_accel_reg, 1, true); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, lin_accel, 6, false); - int16_t x, y, z; - x = y = z = 0; - x = ((int16_t)lin_accel[0]) | (((int16_t)lin_accel[1]) << 8); - y = ((int16_t)lin_accel[2]) | (((int16_t)lin_accel[3]) << 8); - z = ((int16_t)lin_accel[4]) | (((int16_t)lin_accel[5]) << 8); - accel_x = ((float)x) / 100.0; - accel_y = ((float)y) / 100.0; - accel_z = ((float)z) / 100.0; - - uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, &quat_reg, 1, true); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, quat, 8, false); - int16_t w; - w = x = y = z = 0; - w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8); - x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8); - y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8); - z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8); - abs_quaternion_w = ((float)w) / 16384.0; // 2^14 LSB - abs_quaternion_x = ((float)x) / 16384.0; - abs_quaternion_y = ((float)y) / 16384.0; - abs_quaternion_z = ((float)z) / 16384.0; - - Eigen::Quaternion<float> q; - q.w() = abs_quaternion_w; - q.x() = abs_quaternion_x; - q.y() = abs_quaternion_y; - q.z() = abs_quaternion_z; - // q.normalize(); - Eigen::Matrix3f rotation_matrix = q.toRotationMatrix(); - Eigen::Vector3f lin_accel; - abs_lin_accel_x = accel_x* rotation_matrix(0, 0) + accel_y * rotation_matrix(0, 1) + accel_z* rotation_matrix(0, 2); - abs_lin_accel_y = accel_x * rotation_matrix(1, 0) + accel_y * rotation_matrix(1, 1) + accel_z * rotation_matrix(1, 2); - abs_lin_accel_z = -1.0f * (accel_x * rotation_matrix(2, 0) + accel_y * rotation_matrix(2, 1) + accel_z * rotation_matrix(2, 2)); - - printf("Acceleration Vector: %4.2f, %4.2f, %4.2f\n", accel_x, accel_y, accel_z); - printf("Abs Acceleration Vector: %4.2f, %4.2f, %4.2f\n", abs_lin_accel_x, abs_lin_accel_y, abs_lin_accel_z); - printf("Quaternion: %4.2f, %4.2f, %4.2f, %4.2f\n\n\n", abs_quaternion_w, abs_quaternion_x, abs_quaternion_y, abs_quaternion_z); - sleep_ms(1000); - } - - return 0; -} - -void get_calibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) { - uint8_t buf[1] = {BNO055_CALIB_STAT_ADDR}; - uint8_t cal_data = 0x00; - i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false); - i2c_read_blocking(i2c_default, BNO055_ADDRESS, &cal_data, 1, false); - if (sys != NULL) { - *sys = (cal_data >> 6) & 0x03; - } - if (gyro != NULL) { - *gyro = (cal_data >> 4) & 0x03; - } - if (accel != NULL) { - *accel = (cal_data >> 2) & 0x03; - } - if (mag != NULL) { - *mag = cal_data & 0x03; - } -} - diff --git a/tools/read_flash.c b/tools/read_flash.c deleted file mode 100644 index 91c75fb..0000000 --- a/tools/read_flash.c +++ /dev/null @@ -1,78 +0,0 @@ -#include <stdio.h> -#include <inttypes.h> -#include "boards/pico_w.h" -#include "hardware/spi.h" -#include "spi_flash.h" - -int main() { - stdio_init_all(); - getchar(); - // 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); - - uint8_t entry[PACKET_SIZE]; - - // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN); - 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; - } - } - - printf("\nRead Data:\n"); - printf("time,state,board_temp,deploy_percent,altitude,velocity,lin_ax,lin_ay,lin_az,quat_w,quat_x,quat_y,quat_z\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] >> 4; - uint16_t temperature_data = ((uint16_t)(entry[8] & 0x0F) << 8) | ((uint16_t)entry[9]); - const float conversionFactor = 3.3f / (1 << 12); - float tempC = 27.0f - (((float)(temperature_data) * conversionFactor) - 0.706f) / 0.001721f; - - uint8_t deploy_percent = entry[10]; - - float altitude = (float) ((int16_t) ((entry[11] << 8) | entry[12])) + (float) (entry[13] >> 4) * 0.0625; - uint32_t vel_bits = (entry[14] << 24) | (entry[15] << 16) | (entry[16] << 8) | (entry[17]); - float velocity = *(float *)(&vel_bits); - - int16_t ax = ((int16_t)entry[18]) | (((int16_t)entry[19]) << 8); - int16_t ay = ((int16_t)entry[20]) | (((int16_t)entry[21]) << 8); - int16_t az = ((int16_t)entry[22]) | (((int16_t)entry[23]) << 8); - float lax = ((float)ax) / 100.0; - float lay = ((float)ay) / 100.0; - float laz = ((float)az) / 100.0; - - int16_t w, x, y, z; - w = x = y = z = 0; - w = ((int16_t)entry[24]) | (((int16_t)entry[25]) << 8); - x = ((int16_t)entry[26]) | (((int16_t)entry[27]) << 8); - y = ((int16_t)entry[28]) | (((int16_t)entry[29]) << 8); - z = ((int16_t)entry[30]) | (((int16_t)entry[31]) << 8); - float qw = ((float)w) / 16384.0; - float qx = ((float)x) / 16384.0; - float qy = ((float)y) / 16384.0; - float qz = ((float)z) / 16384.0; - - printf("%"PRIu64",%"PRIu8",%04.2f,%"PRIu8",%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f\r\n", \ - now_us, state, tempC, deploy_percent, altitude, velocity, lax, lay, laz, qw, qx, qy, qz); - } -} 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(); + } +} + diff --git a/tools/servo_test.cpp b/tools/servo_test.cpp index c5e8e6e..9c26c25 100644 --- a/tools/servo_test.cpp +++ b/tools/servo_test.cpp @@ -3,16 +3,14 @@ #include "pico/stdio.h" #include "pwm.hpp" -#define MOSFET_PIN 1 - PWM pwm; int main() { stdio_init_all(); // Initialize MOSFET - gpio_init(MOSFET_PIN); - gpio_set_dir(MOSFET_PIN, GPIO_OUT); - gpio_put(MOSFET_PIN, 1); + gpio_init(MICRO_DEFAULT_SERVO_ENABLE); + gpio_set_dir(MICRO_DEFAULT_SERVO_ENABLE, GPIO_OUT); + gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1); pwm.init(); uint8_t duty_cycle = 13; while (1) { |