diff options
52 files changed, 4776 insertions, 179 deletions
@@ -1,4 +1,3 @@ -<<<<<<< HEAD Testing/* .DS_Store .vagrant/* diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..4d3602d --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "include/pico-sdk"] + path = include/pico-sdk + url = https://github.com/raspberrypi/pico-sdk +[submodule "include/eigen"] + path = include/eigen + url = https://gitlab.com/libeigen/eigen.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 28e3426..4973b9d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,51 +1,32 @@ -cmake_minimum_required(VERSION 3.14) -set(CMAKE_SYSTEM_NAME Linux) -set(CMAKE_SYSTEM_PROCESSOR ARM) -set(CMAKE_BUILD_TYPE Debug) -set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) - -# Set the cross-compiler for ARM32 -if(CMAKE_HOST_APPLE) - message(STATUS "Running on an Apple system") - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") - include(CTest) - # Apple Settings - # Musl has many targets, must select one - set(CMAKE_C_COMPILER /opt/homebrew/bin/arm-linux-musleabihf-gcc) - set(CMAKE_CXX_COMPILER /opt/homebrew/bin/arm-linux-musleabihf-g++) -elseif(UNIX) - message(STATUS "Running on an Linux system") - - # Linux Settings - # (arm-linux-musleabihf- is 32 bit, aarch64-linux... is the 64 bit version) - set(CMAKE_C_COMPILER arm-linux-musleabihf-gcc) - set(CMAKE_CXX_COMPILER arm-linux-musleabihf-g++) -endif() +cmake_minimum_required(VERSION 3.12) -# Set the architecture and flags -# set(CMAKE_C_FLAGS "-march=armv7-a+fp") -# set(CMAKE_CXX_FLAGS "-march=armv7-a+fp") +set(PICO_BOARD pico_w) +# Pull in SDK (must be before project) +include(include/pico-sdk/pico_sdk_init.cmake) +include_directories(include/eigen) -# project(Active-Drag-System CXX) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) -# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/out") +project(active-drag-system C CXX ASM) +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) -add_subdirectory(src) +if (PICO_SDK_VERSION_STRING VERSION_LESS "1.3.0") + message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.3.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}") +endif() -if(BUILD_TESTING) - include(CTest) +# Initialize the SDK +pico_sdk_init() - add_subdirectory(test) +add_subdirectory(src) - include(FetchContent) - FetchContent_Declare( - googletest - URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip - ) - # For Windows: Prevent overriding the parent project's compiler/linker settings - set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) +add_compile_options(-Wall + -Wno-format # int != int32_t as far as the compiler is concerned because gcc has int32_t as long int + -Wno-unused-function # we have some for the docs that aren't called + ) +if (CMAKE_C_COMPILER_ID STREQUAL "GNU") + add_compile_options(-Wno-maybe-uninitialized) endif() diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..62b9af6 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,15 @@ +FROM alpine:3.17.0 + +# Install toolchain +RUN apk update && \ + apk upgrade && \ + apk add git \ + python3 \ + py3-pip \ + cmake \ + build-base \ + libusb-dev \ + bsd-compat-headers \ + newlib-arm-none-eabi \ + gcc-arm-none-eabi \ + valgrind
\ No newline at end of file @@ -1,9 +1,13 @@ -# THIS IS THE BRANCH USED FOR SOUTH CAROLINA TEST LAUNCH 02/24/2024 - # Active Drag System (ADS) -This is the main codebase for Rocketry at Virginia Tech's Active Drag System, also known colloquially as the ADS, for the 2023-2024 competition year. It runs primarily on a BeagleBone Black, and its goal is to autonomously control the ADS' deployment during flight. +This is the main codebase for Rocketry at Virginia Tech's Active Drag System, also known colloquially as the ADS, for the 2023-2024 competition year. It runs primarily on a Raspberry Pi Pico, and its goal is to autonomously control the ADS' deployment during flight. -`Eigen` Library, `cmake` and `Google Test` required for successful build. +`Eigen` Library, `cmake`, and `arm-none-eabi-gcc` tooling required for successful build. +## Before Build +```shell +git clone https://github.com/RocketryVT/active-drag-system.git +cd active-drag-system/ +git submodule update --init --recursive +``` ## BUILD ```shell @@ -18,50 +22,31 @@ Enable WSL2 in windows Install Ubuntu 22 LTS from Windows Store ```shell sudo apt update && upgrade -sudo apt install build-essential cmake valgrind crossbuild-essential-armhf -sudo apt install musl musl-tools +sudo apt install build-essential cmake valgrind gcc-arm-none-eabi ``` Then to actually build: ```shell cmake -B build -cmake --build build/ +cmake --build build ``` ## BUILD Alternative (Mac) ```shell -brew install arm-linux-gnueabihf-binutils -# This installs just the arm library, you can remove x86_64 if you want those too -brew install FiloSottile/musl-cross/musl-cross --without-x86_64 --with-arm-hf +brew install arm-none-eabi-gcc ``` To check if installed correctly run: ```shell -ls /opt/homebrew/bin | grep "musl" +ls /opt/homebrew/bin | grep "none" ``` -you should see: +you should see a list including but not limited to: ```shell -arm-linux-musleabihf-gcc -arm-linux-musleabihf-g++ +arm-none-eabi-gcc +arm-none-eabi-g++ ``` - -## RUN +Next: ```shell -scp -r src/ads [email protected]:~/ -ssh [email protected] -./ads -``` - -## TEST -```shell -scp -r test/test_ads [email protected]:~/ -ssh [email protected] -./test_ads +cmake -B build +cmake --build build ``` -## GPIO Pins -The GPIO number is calculated by taking the GPIO chip -number, multiplying it by 32, and then adding the offset. For example, -GPIO1_12 = (1 × 32) + 12 = GPIO 44. - -```shell -/sys/class/gpio/gpio44 = GPIO1_12 -``` +Binary files should be located in build/src/*.uf2 after a successful build. diff --git a/Vagrantfile b/Vagrantfile index 7f542e1..ff65b09 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -5,14 +5,7 @@ apt-get update && apt-get upgrade # install base development tools
apt-get -y install build-essential
-apt-get -y install cmake valgrind
-apt-get -y install crossbuild-essential-armhf
-apt-get -y install libboost-all-dev
-apt-get -y install libasio-dev
-wget https://musl.cc/arm-linux-musleabihf-cross.tgz
-tar -xzvf arm-linux-musleabihf-cross.tgz
-echo 'export PATH="\$PATH:/home/vagrant/arm-linux-musleabihf-cross/bin"' >> .bashrc
-rm -r arm-linux-musleabihf-cross.tgz
+apt-get -y install cmake valgrind gcc-arm-none-eabi
BOOTSTRAP
#-----------------------------------------------------------------------
diff --git a/bbb_setup.sh b/bbb_setup.sh deleted file mode 100755 index 42e008d..0000000 --- a/bbb_setup.sh +++ /dev/null @@ -1,33 +0,0 @@ -#!/bin/sh - -# Change based upon device specifics -IFACE_BB=enp0s20f0u5 -IFACE_SELF=wlan0 -EXE=ads - -echo "Compiling Executeables" -vagrant up -vagrant ssh << EOF - cmake /vagrant; - cmake --build .; - mkdir -p build - cp -r out/* /vagrant/build/ -EOF - -echo "Forwarding Traffic to Beaglebone" -# Forwards all internet traffic requested by beaglebone to proper interface -sudo sh -c "ip link set $IFACE_BB" -sudo sh -c "dhclient $IFACE_BB" - -sudo sh -c "iptables --table nat --append POSTROUTING --out-interface $IFACE_SELF -j MASQUERADE" -sudo sh -c "iptables --append FORWARD --in-interface $IFACE_BB -j ACCEPT" -sudo sh -c "echo 1 > /proc/sys/net/ipv4/ip_forward" - -echo "Copying Executeables to Beaglebone" -scp -r build/* [email protected]:~/; -rm -r build - -echo "Running Main Program" -ssh [email protected] /bin/bash << EOF - ./$EXE; -EOF diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..6c2ece8 --- /dev/null +++ b/build.sh @@ -0,0 +1,3 @@ +#!/bin/bash +cmake -B build +cmake --build build
\ No newline at end of file diff --git a/compose-dev.yaml b/compose-dev.yaml new file mode 100644 index 0000000..81a2ce4 --- /dev/null +++ b/compose-dev.yaml @@ -0,0 +1,14 @@ +services: + app: + build: + context: . + dockerfile: Dockerfile + entrypoint: + - sleep + - infinity + init: true + volumes: + - type: bind + source: /var/run/docker.sock + target: /var/run/docker.sock + diff --git a/include/SimpleKalmanFilter.h b/include/SimpleKalmanFilter.h new file mode 100644 index 0000000..61b682e --- /dev/null +++ b/include/SimpleKalmanFilter.h @@ -0,0 +1,32 @@ +/* + * SimpleKalmanFilter - a Kalman Filter implementation for single variable models. + * Created by Denys Sene, January, 1, 2017. + * Released under MIT License - see LICENSE file for details. + */ + +#ifndef SimpleKalmanFilter_h +#define SimpleKalmanFilter_h + +class SimpleKalmanFilter +{ + +public: + SimpleKalmanFilter(float mea_e, float est_e, float q); + float updateEstimate(float mea); + void setMeasurementError(float mea_e); + void setEstimateError(float est_e); + void setProcessNoise(float q); + float getKalmanGain(); + float getEstimateError(); + +private: + float _err_measure; + float _err_estimate; + float _q; + float _current_estimate = 0; + float _last_estimate = 0; + float _kalman_gain = 0; + +}; + +#endif diff --git a/include/bno055.hpp b/include/bno055.hpp new file mode 100644 index 0000000..7fbac4d --- /dev/null +++ b/include/bno055.hpp @@ -0,0 +1,292 @@ +#pragma once + +#include <vector> +#include <string> + +#include "boards/pico_w.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include "pico/time.h" +#include "pico/types.h" + +#include <Eigen/Geometry> + +//Register addresses and data structs copied from Adafruit implementation +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} bno055_opmode_t; + +/** BNO055 Registers **/ +typedef enum { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR = 0X07, + + /* PAGE0 REGISTER DEFINITION START*/ + BNO055_CHIP_ID_ADDR = 0x00, + BNO055_ACCEL_REV_ID_ADDR = 0x01, + BNO055_MAG_REV_ID_ADDR = 0x02, + BNO055_GYRO_REV_ID_ADDR = 0x03, + BNO055_SW_REV_ID_LSB_ADDR = 0x04, + BNO055_SW_REV_ID_MSB_ADDR = 0x05, + BNO055_BL_REV_ID_ADDR = 0X06, + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, + BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, + BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, + BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, + BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, + BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, + BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, + BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, + BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, + BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, + BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, + BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, + BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, + BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, + BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, + BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR = 0X1A, + BNO055_EULER_H_MSB_ADDR = 0X1B, + BNO055_EULER_R_LSB_ADDR = 0X1C, + BNO055_EULER_R_MSB_ADDR = 0X1D, + BNO055_EULER_P_LSB_ADDR = 0X1E, + BNO055_EULER_P_MSB_ADDR = 0X1F, + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, + BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, + BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, + BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, + BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, + BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, + BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, + BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, + BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, + BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, + BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, + BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, + BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, + BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, + BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, + BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, + BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, + BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, + + /* Temperature data register */ + BNO055_TEMP_ADDR = 0X34, + + /* Status registers */ + BNO055_CALIB_STAT_ADDR = 0X35, + BNO055_SELFTEST_RESULT_ADDR = 0X36, + BNO055_INTR_STAT_ADDR = 0X37, + + BNO055_SYS_CLK_STAT_ADDR = 0X38, + BNO055_SYS_STAT_ADDR = 0X39, + BNO055_SYS_ERR_ADDR = 0X3A, + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR = 0X3B, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A, + + /* Interrupt registers*/ + RST_INT = 0x01, + + NO_MOTION_INT_ADDR = 0x00, + SLOW_NO_MOTION_INT_ADDR = 0x01, + THRESHOLD_INT_ADDR = 0x02, +} bno055_reg_t; + +/** BNO055 power settings */ +typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 +} bno055_powermode_t; + +/** Vector Mappings **/ +typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR +} vector_type_t; + +struct quarternion { + float w; + float x; + float y; + float z; +}; + +struct vector3f { + float x; + float y; + float z; +}; + +struct CALIB_STATUS { + uint8_t sys; + uint8_t gyro; + uint8_t accel; + uint8_t mag; +}; + +extern volatile vector3f linear_acceleration; +extern volatile vector3f acceleration; +extern volatile quarternion abs_quaternion; +extern volatile CALIB_STATUS calib_status; + +extern volatile vector3f euler_angles; +extern volatile vector3f abs_lin_accel; +extern volatile vector3f rot_y_vec; +extern volatile vector3f vel_at_angle; + +extern volatile vector3f accel_gravity; + +extern uint8_t accel[6]; +extern uint8_t quat[8]; + +class BNO055 { + public: + BNO055(); + //Sanity check for factory device ID + void reset_bno055(); + void init(); + void read_lin_accel(); + void read_abs_quaternion(); + void read_euler_angles(); + void read_accel(); + void read_calib_status(); + void calculate_abs_linear_acceleration(); + void accel_to_gravity(); + void quaternion_to_euler(); + void get_rotation_vector(); + void clamp_close_zero(volatile float &val); + + private: + unsigned char bno055_address; + int32_t _sensorID; + bno055_opmode_t default_mode; +}; diff --git a/include/eigen b/include/eigen new file mode 160000 +Subproject 122befe54cc0c31273d9e1caef80b49ad834bf4 diff --git a/include/kalmanfilter.hpp b/include/kalmanfilter.hpp index 2c8398c..8b9e942 100644 --- a/include/kalmanfilter.hpp +++ b/include/kalmanfilter.hpp @@ -1,5 +1,5 @@ #pragma once -#include "eigen3/Eigen/Dense" +#include <Eigen/Dense> #include <iostream> #include <cmath> diff --git a/include/pico-sdk b/include/pico-sdk new file mode 160000 +Subproject 6a7db34ff63345a7badec79ebea3aaef1712f37 diff --git a/include/pwm.hpp b/include/pwm.hpp new file mode 100644 index 0000000..90e31cc --- /dev/null +++ b/include/pwm.hpp @@ -0,0 +1,36 @@ +#include "boards/pico_w.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "hardware/pwm.h" +#include "pico/stdlib.h" +#include "pico/time.h" +#include "pico/types.h" +#include "pico/divider.h" + +#define SERVO_PIN 27 // Servo motor connected to GPIO PIN 32 (GP27) +#define WRAP_VALUE 65535 +#define CLOCK_DIV_RATE 38.15 +#define SERVO_MIN 13 +#define SERVO_MAX 3 +#define SERVO_RANGE -10 + +class PWM { + public: + /** + * @brief Initialize the PWM + * + */ + static void init(); + /** + * @brief Set the duty cycle of our servo + * + * @param duty_cycle_percent from 0 to 100 + */ + static void set_duty_cycle(int duty_cycle_percent); + /** + * @brief Set the servo percent objec to the given percent + * + * @param percent from 0 to 100 + */ + static void set_servo_percent(int percent); +}; diff --git a/include/spi_flash.h b/include/spi_flash.h new file mode 100644 index 0000000..d3c0247 --- /dev/null +++ b/include/spi_flash.h @@ -0,0 +1,55 @@ +#ifndef SPI_FLASH +#define SPI_FLASH + +#include <stdint.h> +#include "hardware/spi.h" +#include "boards/pico_w.h" +#include "pico/stdlib.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define FLASH_PAGE_SIZE 256 +#define FLASH_NUM_PAGES 32768 +#define FLASH_SECTOR_SIZE 4096 +#define FLASH_BLOCK_SIZE 65536 +#define FLASH_PHYS_SIZE (FLASH_PAGE_SIZE * FLASH_NUM_PAGES) + +#define FLASH_CMD_PAGE_PROGRAM 0x02 +#define FLASH_CMD_READ 0x03 +#define FLASH_CMD_STATUS 0x05 +#define FLASH_CMD_WRITE_EN 0x06 +#define FLASH_CMD_SECTOR_ERASE 0x20 +#define FLASH_CMD_BLOCK_ERASE 0xD8 +#define FLASH_CMD_CHIP_ERASE 0xC7 + +#define FLASH_STATUS_BUSY_MASK 0x01 + +// #define FLASH_TEST + +#define PACKET_SIZE 32 + + +static uint8_t page_buffer[FLASH_PAGE_SIZE]; +static uint32_t base_addr = 0; + +void write_entry(uint8_t* data_entry); + +void __not_in_flash_func(flash_read)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t *dest, size_t len); + + +void __not_in_flash_func(flash_write_enable)(spi_inst_t *spi, uint cs_pin); +void __not_in_flash_func(flash_page_program)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src); +void __not_in_flash_func(flash_write)(spi_inst_t *spi, uint cs_pin, uint32_t addr, uint8_t* src, size_t size); + +void __not_in_flash_func(flash_wait_done)(spi_inst_t *spi, uint cs_pin); + +void __not_in_flash_func(flash_sector_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr); +void __not_in_flash_func(flash_block_erase)(spi_inst_t *spi, uint cs_pin, uint32_t addr); +void __not_in_flash_func(flash_erase)(spi_inst_t *spi, uint cs_pin); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/include/state_machine.hpp b/include/state_machine.hpp new file mode 100644 index 0000000..874fc08 --- /dev/null +++ b/include/state_machine.hpp @@ -0,0 +1,26 @@ +#ifndef STATE_MACHINE_H +#define STATE_MACHINE_H + +#include <stdint.h> + +extern volatile float altitude = 0.0f; +extern volatile float prev_altitude = 0.0f; +extern volatile float velocity; +extern volatile state_t state; +extern volatile float threshold_altitude; +extern volatile float threshold_velocity; +extern volatile uint8_t deployment_percent; +extern volatile vector3f linear_acceleration; +extern volatile vector3f acceleration; +extern volatile quarternion abs_quaternion; +extern volatile vector3f velocity_vector; +extern volatile vector3f euler_angles; +extern volatile vector3f abs_lin_accel; +extern volatile vector3f prev_abs_lin_accel; +extern volatile vector3f rot_y_vec; +extern volatile vector3f vel_at_angle; +extern volatile vector3f accel_gravity; +extern volatile CALIB_STATUS calib_status; + +#endif + diff --git a/include/unused/actuationPlan.hpp b/include/unused/actuationPlan.hpp new file mode 100644 index 0000000..8d1ef26 --- /dev/null +++ b/include/unused/actuationPlan.hpp @@ -0,0 +1,36 @@ +#pragma once +#include <algorithm> +#include <ctime> +#include "surfaceFitModel.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" + +class ActuationPlan { + + private: + SurfaceFitModel sFitModel; + + public: + + /** + * @brief Construct a new Actuation Plan object + * + */ + ActuationPlan(); + + /** + * @brief Construct a new Actuation Plan object + * + * @param sFitModel + */ + ActuationPlan(SurfaceFitModel sFitModel); + + /** + * @brief Run the Fin Actuation Plan. + * Adjusts the fin angle values depending on the current vehicle state during the launch + * + * @param rocket Provides current rocket status and hold updated fin angle value. + */ + void runPlan(Vehicle& rocket); +};
\ No newline at end of file diff --git a/include/unused/actuator.hpp b/include/unused/actuator.hpp new file mode 100644 index 0000000..e7c4120 --- /dev/null +++ b/include/unused/actuator.hpp @@ -0,0 +1,30 @@ +#pragma once + + +class Actuator { + + private: + + + public: + + /** + * @brief Initialize the actuator. + * + * @param data Data for initializing the actuator + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) = 0; + + /** + * @brief Pass data to the actuator. + * + * @param data Data to sent to the actuator + * + * @return true Actuator write Success + * @return false Actuator write Failure + */ + virtual bool writeData(void* data) = 0; +};
\ No newline at end of file diff --git a/include/unused/ads.hpp b/include/unused/ads.hpp new file mode 100644 index 0000000..b16c4fb --- /dev/null +++ b/include/unused/ads.hpp @@ -0,0 +1,66 @@ +#pragma once +#include <iostream> +#include <exception> +#include <stdexcept> +#include <chrono> +#include <thread> +#include "kalmanfilter.hpp" +#include "logger.hpp" +#include "actuationPlan.hpp" +#include "rocketUtils.hpp" +#include "sensorIMU.hpp" +#include "sensorAltimeter.hpp" +#include "motor.hpp" +#include "eigen3/Eigen/Dense" + +using namespace Eigen; + +class ADS { + + private: + + KalmanFilter kf; + ActuationPlan plan; + + IMUSensor imu; + AltimeterSensor altimeter; + Motor motor; + Vehicle rocket; + + /** + * @brief Logs a summary of all pertinent current rocket data + * (e.g. Altitude, Velocity, Acceleration) + */ + virtual void logSummary(); + + /** + * @brief Performs a routine to calculate the average altitude + * while the vehicle is waiting on the pad. + */ + virtual void updateOnPadAltitude(); + + /** + * @brief Update the vehicle with the current sensor (IMU & Altimeter) readings + */ + virtual void updateSensorData(); + + /** + * @brief Update the rocket state based on its current telemetry readings. + * Also Log pertinent telemetry and rocket state data + */ + virtual void updateRocketState(); + + public: + + /** + * @brief Construct a new ADS object + * + * @param plan The Actuation Plan for the Rocket + */ + ADS(ActuationPlan plan); + + /** + * @brief Run the full active drag system from launch to landing. + */ + virtual void run(); +};
\ No newline at end of file diff --git a/include/unused/logger.hpp b/include/unused/logger.hpp new file mode 100644 index 0000000..90fa05a --- /dev/null +++ b/include/unused/logger.hpp @@ -0,0 +1,83 @@ +#pragma once +#include <iostream> +#include <fstream> +#include <ctime> + +class Logger { + + private: + std::fstream file; + std::string filename; + time_t t; + tm* now; + bool file_open; + + std::string infoTag = "-> [INFO]: "; + std::string errorTag = "-> [ERROR]: "; + std::string months[12] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sept", "Oct", "Nov", "Dec"}; + std::string days[7] = {"Sun", "Mon", "Tues", "Wed", "Thur", "Frid", "Sat"}; + + /** + * @brief Create formatted current-date tag + * + * @return string + */ + std::string getDate(); + + /** + * @brief Create formatted current-time tag + * + * @return string + */ + std::string getTime(); + + + public: + + /** + * @brief + * + */ + static Logger& Get(); + + /** + * @brief Open a Log File for writing + * + * @param _filename Name of file to open + * @return true Successful Open + * @return false Unsuccessful Open + */ + bool openLog(std::string _filename); + + /** + * @brief Close the Log File + * + */ + void closeLog(); + + /** + * @brief Write data to Log file + * + * @param data Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool log(std::string data); + + /** + * @brief Write error data to Log file + * + * @param data Error Data to log + * @return true Data Successfully Logged + * @return false Data Logging Unsuccessful + */ + bool logErr(std::string data); + + /** + * @brief Print Log Data to Terminal + * + * @return true Successful Print + * @return false Unsuccessful Print + */ + bool printLog(); +};
\ No newline at end of file diff --git a/include/unused/motor.hpp b/include/unused/motor.hpp new file mode 100644 index 0000000..cb95b6a --- /dev/null +++ b/include/unused/motor.hpp @@ -0,0 +1,36 @@ +#pragma once +#include "actuator.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" + +class Motor : public Actuator { + + private: + + + public: + + /** + * @brief Construct a new Motor object + * + */ + Motor(); + + /** + * @brief Initialize the motor. + * + * @param data Data for initializing the motor + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init(void* data) override; + + /** + * @brief Write data to the motor. + * + * @param data Data to be writen to the motor + * @return true Motor write Success + * @return false Motor write Failure + */ + virtual bool writeData(void* data) override; +};
\ No newline at end of file diff --git a/include/unused/rocketUtils.hpp b/include/unused/rocketUtils.hpp new file mode 100644 index 0000000..65b9cea --- /dev/null +++ b/include/unused/rocketUtils.hpp @@ -0,0 +1,121 @@ +#pragma once +#include <iostream> +#include <vector> +#include <ctime> +#include <vector> +#include <iomanip> +#include <sstream> +#include <fstream> + +// Deployment angle limits +#define MAX_ANGLE 100 +#define MIN_ANGLE 120 + +// Altimeter initialization count limit +#define COUNT_LIMIT 50 + +// Constants +#define G_0 9.8066 + +// Threshold limits +#define BOOST_ACCEL_THRESH 3 +#define BOOST_HEIGHT_THRESH 20 +#define GLIDE_ACCEL_THRESH 0.5 + +#define ALTI_DEPL_THRESHOLD 0.5 +#define RATE_ALTI_DEPL 1/50 +#define FSM_DONE_SURFACE_ALTITUDE 200 +#define APOGEE_FSM_CHANGE 3 + +#define INIT_DEPLOYMENT 0 + +#define TIME_BO 8 +#define TIME_APO 25 +#define TIME_END 120 + +#define PAD_PRESSURE 102250 + +#define DUTY_MAX 14.5 +#define DUTY_MIN 3 + +#define LAUNCH_DATE "4-15-2023" +#define LOG_FILENAME "DataLog_" LAUNCH_DATE ".txt" + +#define LED_GAP_TIME 0.5 +#define LED_ONE_PATH "/sys/class/leds/beaglebone:green:usr1" +#define LED_BRIGHTNESS_FILE "brightness" +#define LED_FILENAME LED_ONE_PATH LED_BRIGHTNESS_FILE + +#define TEST_MODE false + + +enum VehicleState {ON_PAD, BOOST, GLIDE, APOGEE, DONE}; +extern std::string state_for_log[5]; + +struct Vehicle { + + int status; + + std::vector<double> acceleration; + std::vector<double> linear_acceleration; + + double apogee_altitude; + double previous_altitude; + double current_altitude; + double filtered_altitude; + + double filtered_velocity; + + double deployment_angle; + + bool imuInitFail; + bool imuReadFail; + bool altiInitFail; + bool altiReadFail; + + double ON_PAD_altitude; + bool ON_PAD_fail; + + double duty_span; + + double dt; + + int led_brightness; + + time_t start_time; + time_t fail_time; // For failure termination + time_t liftoff_time; + time_t relog_time; + time_t deploy_time; // NOT INITIALIZED YET + time_t loop_time; + time_t led_time; +}; + +/** + * @brief Convert fin deployment percentage to fin rotation angle + * + * @param percentage Fin deployment percentage + * @return double + */ +double deploy_percentage_to_angle(double percentage); + +/** + * @brief Set the decimal precision of the given data, and return it + * as a formatted string with a prefix containing a relevant description of the data. + * + * @param prefix Identifying or clarifying information about the loggef data + * @param data Data to Log + * @param precision The decimal precision value for the data + * + * @return A string with the formatted data. + */ +std::string format_data(std::string prefix, double data, int precision); + +/** + * @brief Blink Beaglebone LED 1 + * + * @param vehicle Holds settings pertinent to the Beaglebone LED + * @return true Successful Blink + * @return false Unsuccessful Blink + */ +bool led_out(Vehicle *vehicle); diff --git a/include/unused/sensorAltimeter.hpp b/include/unused/sensorAltimeter.hpp new file mode 100644 index 0000000..aa3a3cd --- /dev/null +++ b/include/unused/sensorAltimeter.hpp @@ -0,0 +1,165 @@ +#pragma once +#include "sensorI2C.hpp" +#include <string> + +/* +Header file for driver for MPL3115a2 breakout, based heavily on https://github.com/adafruit/Adafruit_MPL3115A2_Library/tree/master +Based on register declarations found https://cdn-shop.adafruit.com/datasheets/1893_datasheet.pdf +Designed using subclass for I2C handler for Beaglebone Black +*/ + +class AltimeterSensor : public sensorI2C { + + + private: + double internalTemperature = 0; + double internalAltitude = 0; + + //ENUMS COPIED DIRECTLY FROM ADAFRUIT IMPLEMENTATION + /** MPL3115A2 registers **/ + enum { + MPL3115A2_REGISTER_STATUS = (0x00), + + MPL3115A2_REGISTER_PRESSURE_MSB = (0x01), + MPL3115A2_REGISTER_PRESSURE_CSB = (0x02), + MPL3115A2_REGISTER_PRESSURE_LSB = (0x03), + + MPL3115A2_REGISTER_TEMP_MSB = (0x04), + MPL3115A2_REGISTER_TEMP_LSB = (0x05), + + MPL3115A2_REGISTER_DR_STATUS = (0x06), + + MPL3115A2_OUT_P_DELTA_MSB = (0x07), + MPL3115A2_OUT_P_DELTA_CSB = (0x08), + MPL3115A2_OUT_P_DELTA_LSB = (0x09), + + MPL3115A2_OUT_T_DELTA_MSB = (0x0A), + MPL3115A2_OUT_T_DELTA_LSB = (0x0B), + + MPL3115A2_WHOAMI = (0x0C), + //This is hard-coded in the device from the factory + MPL3115A2_WHOAMI_EXPECTED = (0xC4), + + MPL3115A2_BAR_IN_MSB = (0x14), + MPL3115A2_BAR_IN_LSB = (0x15), + + MPL3115A2_OFF_H = (0x2D), + }; + + /** MPL3115A2 status register BITS **/ + enum { + MPL3115A2_REGISTER_STATUS_TDR = 0x02, + MPL3115A2_REGISTER_STATUS_PDR = 0x04, + MPL3115A2_REGISTER_STATUS_PTDR = 0x08, + }; + + /** MPL3115A2 PT DATA register BITS **/ + enum { + MPL3115A2_PT_DATA_CFG = 0x13, + MPL3115A2_PT_DATA_CFG_TDEFE = 0x01, + MPL3115A2_PT_DATA_CFG_PDEFE = 0x02, + MPL3115A2_PT_DATA_CFG_DREM = 0x04, + }; + + /** MPL3115A2 control registers **/ + enum { + MPL3115A2_CTRL_REG1 = (0x26), + MPL3115A2_CTRL_REG2 = (0x27), + MPL3115A2_CTRL_REG3 = (0x28), + MPL3115A2_CTRL_REG4 = (0x29), + MPL3115A2_CTRL_REG5 = (0x2A), + }; + + /** MPL3115A2 control register BITS **/ + enum { + MPL3115A2_CTRL_REG1_SBYB = 0x01, + MPL3115A2_CTRL_REG1_OST = 0x02, + MPL3115A2_CTRL_REG1_RST = 0x04, + MPL3115A2_CTRL_REG1_RAW = 0x40, + MPL3115A2_CTRL_REG1_ALT = 0x80, + MPL3115A2_CTRL_REG1_BAR = 0x00, + }; + + /** MPL3115A2 oversample values **/ + enum { + MPL3115A2_CTRL_REG1_OS1 = 0x00, + MPL3115A2_CTRL_REG1_OS2 = 0x08, + MPL3115A2_CTRL_REG1_OS4 = 0x10, + MPL3115A2_CTRL_REG1_OS8 = 0x18, + MPL3115A2_CTRL_REG1_OS16 = 0x20, + MPL3115A2_CTRL_REG1_OS32 = 0x28, + MPL3115A2_CTRL_REG1_OS64 = 0x30, + MPL3115A2_CTRL_REG1_OS128 = 0x38, + }; + + /** MPL3115A2 measurement modes **/ + typedef enum { + MPL3115A2_BAROMETER = 0, + MPL3115A2_ALTIMETER, + } mpl3115a2_mode_t; + + /** MPL3115A2 measurement types **/ + typedef enum { + MPL3115A2_PRESSURE, + MPL3115A2_ALTITUDE, + MPL3115A2_TEMPERATURE, + } mpl3115a2_meas_t; + + //This never actually gets used, and I can't find anything in the datasheet about it?? + #define MPL3115A2_REGISTER_STARTCONVERSION (0x12) ///< start conversion + + //Store current operating mode, sent to device during startup procedure + //This is why an enum is used rather than raw #define statements + mpl3115a2_mode_t currentMode; + + //Struct for storing ctrl register contents, copied from adafruit implementation + typedef union { + struct { + uint8_t SBYB : 1; + uint8_t OST : 1; + uint8_t RST : 1; + uint8_t OS : 3; + uint8_t RAW : 1; + uint8_t ALT : 1; + } bit; + uint8_t reg; + } CTRL_REG_1_STRUCT; + //Create instance of this register config to use during device startup and operation + CTRL_REG_1_STRUCT ctrl_reg1; + + public: + + /** + * @brief Construct a new Altimeter Sensor object + * + */ + AltimeterSensor(std::string I2C_FILE); + + /** + * @brief Initialize the Altimeter + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data getters and setters + // double getPressure(); + double getAltitude(); + double getTemperature(); + double setSeaLevelPressure(double pressure); + + //Data and mode handlers + //Use altimeter mode by default as this is what rocket logger wants + void setMode(mpl3115a2_mode_t mode = MPL3115A2_ALTIMETER); + void requestOneShotReading(); + bool isNewDataAvailable(); + void updateCurrentDataBuffer(); + + +}; + + + + diff --git a/include/unused/sensorI2C.hpp b/include/unused/sensorI2C.hpp new file mode 100644 index 0000000..bd497a5 --- /dev/null +++ b/include/unused/sensorI2C.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <sys/ioctl.h> +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <cstdint> +#include <string> + +class sensorI2C { + + + //Predominantly used for I2C handler functions; implement high-level functions in sensor classes + //Implemented as a combination of Jazz' implementation and Derek Malloy's code + public: + + //Initial single byte write, used at beginning of read operation + //Returns 0 if successful, -1 if not + int initialWrite(unsigned char registerAddress) { + unsigned char *convertedAddressBuffer = new unsigned char[1]; + convertedAddressBuffer[0] = registerAddress; + //Expect 1 byte response from 1 byte write + if (write(i2c_bus, convertedAddressBuffer, 1) != 1) { + fprintf(stderr, "ERROR DOING INITIAL READ TRANSACTION WRITE TO REGISTER %x FOR DEVICE %x\n", registerAddress, deviceAddress); + return 0; + } + return 1; + } + + int writeRegister(unsigned char registerAddress, unsigned char value) { + //initialWrite() not used here because it's easier to just pack it into one buffer for file writing + unsigned char writeBuffer[2]; + writeBuffer[0] = registerAddress; + writeBuffer[1] = value; + + //Expect 2 byte output + if (write(i2c_bus, writeBuffer, 2) != 2) { + //These error messages are kind of obtuse but I'd rather have too much information than not enough + fprintf(stderr, "ERROR WRITING %x TO REGISTER %x ON DEVICE %x\n", value, registerAddress, deviceAddress); + return -1; + } + return 0; + } + + //Could probably be uint8_t but Derek Malloy does it with unsigned chars and that's what worked during testing so I don't want to touch it + unsigned char readSingleRegister(unsigned char registerAddress) { + printf("reg addr: %X\n", registerAddress); + + initialWrite(registerAddress); + unsigned char* readBuffer = new unsigned char[1]; + if (read(i2c_bus, readBuffer, 1) != 1){ + fprintf(stderr, "FAILED TO READ VALUE FROM REGISTER %x ON DEVICE %x\n", registerAddress, deviceAddress); + return -1; + } + printf("readBuffer: %X\n", readBuffer[0]); + return readBuffer[0]; + } + + unsigned char* readMultipleRegisters(unsigned char startingRegisterAddress, int numberOfRegisters) { + initialWrite(startingRegisterAddress); + unsigned char* readBuffer = new unsigned char[numberOfRegisters]; + if (read(i2c_bus, readBuffer, numberOfRegisters) != numberOfRegisters) { + fprintf(stderr, "ERROR TRYING TO READ %d REGISTERS STARTING AT ADDRESS %x ON DEVICE %x\n", + numberOfRegisters, startingRegisterAddress, deviceAddress); + } + return readBuffer; + } + + + //Intakes device address and file + //Private because IT'S ASSUMED PROGRAMMER WILL CALL THIS METHOD DURING INIT() ROUTINE + int setupI2C(std::string I2C_FILE) { + // Open i2c driver file + i2c_bus = open("/dev/i2c-2", O_RDWR); + if(i2c_bus < 0){ + fprintf(stderr, "FAILED TO OPEN I2C BUS USING FILE %s\n", "/dev/i2c-2"); + close(i2c_bus); + return -1; + } + + // Identify slave device address (MODIFIED FROM INITIAL IMPLEMENTATION, USES INTERNAL deviceAddress INSTEAD OF PARAMETER) + if(ioctl(i2c_bus, I2C_SLAVE, deviceAddress) < 0){ + fprintf(stderr, "FAILED TO CONNECT TO DEVICE AT ADDRESS %x VIA I2C\n", deviceAddress); + close(i2c_bus); + return -1; + } + return 0; + } + + /** + * @brief Initialize the sensor. + * + * @param data Data for initializing the sensor + * + * @return true Initialization Success + * @return false Initialization Failure + */ + virtual bool init() = 0; + + unsigned char deviceAddress; + int i2c_bus; + std::string I2C_FILE; +}; diff --git a/include/unused/sensorIMU.hpp b/include/unused/sensorIMU.hpp new file mode 100644 index 0000000..caddbe4 --- /dev/null +++ b/include/unused/sensorIMU.hpp @@ -0,0 +1,312 @@ +#pragma once +#include <vector> +#include "sensorI2C.hpp" +#include "logger.hpp" +#include "rocketUtils.hpp" +#include "imumath.h" +#include <string> + +struct IMUData { + std::vector<double> acceleration[3]; + std::vector<double> linear_acceleration[3]; +}; + +//Register addresses and data structs copied from Adafruit implementation +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} adafruit_bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} adafruit_bno055_opmode_t; +class IMUSensor : public sensorI2C { + + private: + deviceAddress = BNO055_ADDRESS_A; + + adafruit_bno055_opmode_t default_mode = OPERATION_MODE_NDOF; + /** BNO055 Registers **/ + typedef enum { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR = 0X07, + + /* PAGE0 REGISTER DEFINITION START*/ + BNO055_CHIP_ID_ADDR = 0x00, + BNO055_ACCEL_REV_ID_ADDR = 0x01, + BNO055_MAG_REV_ID_ADDR = 0x02, + BNO055_GYRO_REV_ID_ADDR = 0x03, + BNO055_SW_REV_ID_LSB_ADDR = 0x04, + BNO055_SW_REV_ID_MSB_ADDR = 0x05, + BNO055_BL_REV_ID_ADDR = 0X06, + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, + BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, + BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, + BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, + BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, + BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, + BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, + BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, + BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, + BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, + BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, + BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, + BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, + BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, + BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, + BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR = 0X1A, + BNO055_EULER_H_MSB_ADDR = 0X1B, + BNO055_EULER_R_LSB_ADDR = 0X1C, + BNO055_EULER_R_MSB_ADDR = 0X1D, + BNO055_EULER_P_LSB_ADDR = 0X1E, + BNO055_EULER_P_MSB_ADDR = 0X1F, + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, + BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, + BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, + BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, + BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, + BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, + BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, + BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, + BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, + BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, + BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, + BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, + BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, + BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, + BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, + BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, + BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, + BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, + + /* Temperature data register */ + BNO055_TEMP_ADDR = 0X34, + + /* Status registers */ + BNO055_CALIB_STAT_ADDR = 0X35, + BNO055_SELFTEST_RESULT_ADDR = 0X36, + BNO055_INTR_STAT_ADDR = 0X37, + + BNO055_SYS_CLK_STAT_ADDR = 0X38, + BNO055_SYS_STAT_ADDR = 0X39, + BNO055_SYS_ERR_ADDR = 0X3A, + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR = 0X3B, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A + } adafruit_bno055_reg_t; + + /** BNO055 power settings */ + typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 + } adafruit_bno055_powermode_t; + + /** Remap settings **/ + typedef enum { + REMAP_CONFIG_P0 = 0x21, + REMAP_CONFIG_P1 = 0x24, // default + REMAP_CONFIG_P2 = 0x24, + REMAP_CONFIG_P3 = 0x21, + REMAP_CONFIG_P4 = 0x24, + REMAP_CONFIG_P5 = 0x21, + REMAP_CONFIG_P6 = 0x21, + REMAP_CONFIG_P7 = 0x24 + } adafruit_bno055_axis_remap_config_t; + + /** Remap Signs **/ + typedef enum { + REMAP_SIGN_P0 = 0x04, + REMAP_SIGN_P1 = 0x00, // default + REMAP_SIGN_P2 = 0x06, + REMAP_SIGN_P3 = 0x02, + REMAP_SIGN_P4 = 0x03, + REMAP_SIGN_P5 = 0x01, + REMAP_SIGN_P6 = 0x07, + REMAP_SIGN_P7 = 0x05 + } adafruit_bno055_axis_remap_sign_t; + + /** A structure to represent revisions **/ + typedef struct { + uint8_t accel_rev; /**< acceleration rev */ + uint8_t mag_rev; /**< magnetometer rev */ + uint8_t gyro_rev; /**< gyroscrope rev */ + uint16_t sw_rev; /**< SW rev */ + uint8_t bl_rev; /**< bootloader rev */ + } adafruit_bno055_rev_info_t; + + /** Vector Mappings **/ + typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR + } adafruit_vector_type_t; + + int32_t _sensorID; + adafruit_bno055_opmode_t currentMode; + + public: + /** + * @brief Construct a new IMUSensor object + * + */ + IMUSensor(std::string I2C_FILE); + + /** + * @brief Inititlize the IMU + * + * @param data Data for initializing the sensor + * @return true Initialization Success + * @return false Initialization Failure + */ + bool init() override; + + //Data handlers, copied from adafruit implementation (not all of them) + void setModeTemp(adafruit_bno055_opmode_t mode); + void setModeHard(adafruit_bno055_opmode_t mode); + adafruit_bno055_opmode_t getMode(); + + imu::Vector<3> getVector(adafruit_vector_type_t vector_type); + imu::Quaternion getQuat(); + int8_t getTemp(); + + //Configuration and status getters/setters + void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode); + void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign); + void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, + uint8_t *system_error); + void getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, + uint8_t *mag); + + /* Functions to deal with raw calibration data */ + bool getSensorOffsets(uint8_t *calibData); + bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type); + void setSensorOffsets(const uint8_t *calibData); + void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); + bool isFullyCalibrated(); + + /* Power managments functions */ + void enterSuspendMode(); + void enterNormalMode(); + +};
\ No newline at end of file diff --git a/include/unused/surfaceFitModel.hpp b/include/unused/surfaceFitModel.hpp new file mode 100644 index 0000000..303633f --- /dev/null +++ b/include/unused/surfaceFitModel.hpp @@ -0,0 +1,32 @@ +#pragma once +#include <cmath> +#include "eigen3/Eigen/Dense" + + +using namespace Eigen; + +#define X_DEGREE 4 // Highest x-degree of current Surface Fit Model +#define Y_DEGREE 3 // Highest y-degree of current Surface Fit Model + +class SurfaceFitModel { + + private: + MatrixXd p; // Polynomial values + + public: + + /** + * @brief Construct a new Surface Fit Model object + * + */ + SurfaceFitModel(); + + /** + * @brief + * + * @param x + * @param y + * @return double + */ + double getFit(double x, double y); +}; diff --git a/src/AltEst/algebra.cpp b/src/AltEst/algebra.cpp new file mode 100644 index 0000000..653c3b9 --- /dev/null +++ b/src/AltEst/algebra.cpp @@ -0,0 +1,292 @@ +/* + algebra.cpp: This file contains a number of utilities useful for handling + 3D vectors + + This work is an adaptation from vvector.h, written by Linas Vepstras. The + original code can be found at: + + https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h + + HISTORY: + Written by Linas Vepstas, August 1991 + Added 2D code, March 1993 + Added Outer products, C++ proofed, Linas Vepstas October 1993 + Adapted for altitude estimation tasks by Juan Gallostra June 2018 +*/ + +//#include <cmath> +#include <stdio.h> + +#include "algebra.h" + +// Copy 3D vector +void copyVector(float b[3],float a[3]) +{ + b[0] = a[0]; + b[1] = a[1]; + b[2] = a[2]; +} + + +// Vector difference +void subtractVectors(float v21[3], float v2[3], float v1[3]) +{ + v21[0] = v2[0] - v1[0]; + v21[1] = v2[1] - v1[1]; + v21[2] = v2[2] - v1[2]; +} + +// Vector sum +void sumVectors(float v21[3], float v2[3], float v1[3]) +{ + v21[0] = v2[0] + v1[0]; + v21[1] = v2[1] + v1[1]; + v21[2] = v2[2] + v1[2]; +} + +// scalar times vector +void scaleVector(float c[3],float a, float b[3]) +{ + (c)[0] = a*b[0]; + (c)[1] = a*b[1]; + (c)[2] = a*b[2]; +} + +// accumulate scaled vector +void accumulateScaledVector(float c[3], float a, float b[3]) +{ + (c)[0] += a*b[0]; + (c)[1] += a*b[1]; + (c)[2] += a*b[2]; +} + +// Vector dot product +void dotProductVectors(float * c, float a[3], float b[3]) +{ + *c = a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; +} + +// Vector length +void vectorLength(float * len, float a[3]) +{ + float tmp; + tmp = a[0]*a[0] + a[1]*a[1]+a[2]*a[2]; + *len = sqrt(tmp); +} + +// Normalize vector +void normalizeVector(float a[3]) +{ + float len; + vectorLength(& len,a); + if (len != 0.0) { + len = 1.0 / len; + a[0] *= len; + a[1] *= len; + a[2] *= len; + } +} + +// 3D Vector cross product yeilding vector +void crossProductVectors(float c[3], float a[3], float b[3]) +{ + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; +} + +// initialize matrix +void identityMatrix3x3(float m[3][3]) +{ + m[0][0] = 1.0; + m[0][1] = 0.0; + m[0][2] = 0.0; + + m[1][0] = 0.0; + m[1][1] = 1.0; + m[1][2] = 0.0; + + m[2][0] = 0.0; + m[2][1] = 0.0; + m[2][2] = 1.0; +} + +// matrix copy +void copyMatrix3x3(float b[3][3], float a[3][3]) +{ + b[0][0] = a[0][0]; + b[0][1] = a[0][1]; + b[0][2] = a[0][2]; + + b[1][0] = a[1][0]; + b[1][1] = a[1][1]; + b[1][2] = a[1][2]; + + b[2][0] = a[2][0]; + b[2][1] = a[2][1]; + b[2][2] = a[2][2]; +} + +// matrix transpose +void transposeMatrix3x3(float b[3][3], float a[3][3]) +{ + b[0][0] = a[0][0]; + b[0][1] = a[1][0]; + b[0][2] = a[2][0]; + + b[1][0] = a[0][1]; + b[1][1] = a[1][1]; + b[1][2] = a[2][1]; + + b[2][0] = a[0][2]; + b[2][1] = a[1][2]; + b[2][2] = a[2][2]; +} + +// multiply matrix by scalar +void scaleMatrix3x3(float b[3][3], float s, float a[3][3]) +{ + b[0][0] = (s) * a[0][0]; + b[0][1] = (s) * a[0][1]; + b[0][2] = (s) * a[0][2]; + + b[1][0] = (s) * a[1][0]; + b[1][1] = (s) * a[1][1]; + b[1][2] = (s) * a[1][2]; + + b[2][0] = (s) * a[2][0]; + b[2][1] = (s) * a[2][1]; + b[2][2] = (s) * a[2][2]; +} + +// multiply matrix by scalar and add result to another matrix +void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3]) +{ + b[0][0] += s * a[0][0]; + b[0][1] += s * a[0][1]; + b[0][2] += s * a[0][2]; + + b[1][0] += s * a[1][0]; + b[1][1] += s * a[1][1]; + b[1][2] += s * a[1][2]; + + b[2][0] += s * a[2][0]; + b[2][1] += s * a[2][1]; + b[2][2] += s * a[2][2]; +} + +// matrix product +// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y] +void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3]) +{ + c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; + c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; + c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; + + c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; + c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; + c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; + + c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; + c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; + c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; +} + +// matrix times vector +void matrixDotVector3x3(float p[3], float m[3][3], float v[3]) +{ + p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; + p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; + p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; +} + +// determinant of matrix +// Computes determinant of matrix m, returning d +void determinant3x3(float * d, float m[3][3]) +{ + *d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); + *d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); + *d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); +} + +// adjoint of matrix +// Computes adjoint of matrix m, returning a +// (Note that adjoint is just the transpose of the cofactor matrix) +void adjoint3x3(float a[3][3], float m[3][3]) +{ + a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; + a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); + a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; + a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); + a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; + a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); + a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; + a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); + a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]; +} + +// compute adjoint of matrix and scale +// Computes adjoint of matrix m, scales it by s, returning a +void scaleAdjoint3x3(float a[3][3], float s, float m[3][3]) +{ + a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); + a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); + a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); + + a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); + a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); + a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); + + a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); + a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); + a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); +} + +// inverse of matrix +// Compute inverse of matrix a, returning determinant m and +// inverse b +void invert3x3(float b[3][3], float a[3][3]) +{ + float tmp; + determinant3x3(& tmp, a); + tmp = 1.0 / (tmp); + scaleAdjoint3x3(b, tmp, a); +} + +// skew matrix from vector +void skew(float a[3][3], float v[3]) +{ + a[0][1] = -v[2]; + a[0][2] = v[1]; + a[1][2] = -v[0]; + a[1][0] = v[2]; + a[2][0] = -v[1]; + a[2][1] = v[0]; + // set diagonal to 0 + a[0][0] = 0.0; + a[1][1] = 0.0; + a[2][2] = 0.0; +} + +void printMatrix3X3(float mmm[3][3]) +{ + int i,j; + printf ("matrix mmm is \n"); + if (mmm == NULL) { + printf (" Null \n"); + } else { + for (i=0; i<3; i++) { + for (j=0; j<3; j++) { + printf ("%f ", mmm[i][j]); + } + printf (" \n"); + } + } +} + +void vecPrint(float a[3]) +{ + float len; + vectorLength(& len, a); + printf(" a is %f %f %f length of a is %f \n", a[0], a[1], a[2], len); +} diff --git a/src/AltEst/algebra.h b/src/AltEst/algebra.h new file mode 100644 index 0000000..382103e --- /dev/null +++ b/src/AltEst/algebra.h @@ -0,0 +1,96 @@ +/* + algebra.h: This file contains a number of utilities useful for handling + 3D vectors + + This work is an adaptation from vvector.h, written by Linas Vepstras. The + original code can be found at: + + https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h + + HISTORY: + Written by Linas Vepstas, August 1991 + Added 2D code, March 1993 + Added Outer products, C++ proofed, Linas Vepstas October 1993 + Adapted for altitude estimation tasks by Juan Gallostra June 2018 + Separated .h, .cpp by Simon D. Levy July 2018 +*/ + +#pragma once + +//#include <cmath> +#include <math.h> + +// Copy 3D vector +void copyVector(float b[3],float a[3]); + + +// Vector difference +void subtractVectors(float v21[3], float v2[3], float v1[3]); + +// Vector sum +void sumVectors(float v21[3], float v2[3], float v1[3]); + +// scalar times vector +void scaleVector(float c[3],float a, float b[3]); + +// accumulate scaled vector +void accumulateScaledVector(float c[3], float a, float b[3]); + +// Vector dot product +void dotProductVectors(float * c, float a[3], float b[3]); + +// Vector length +void vectorLength(float * len, float a[3]); + +// Normalize vector +void normalizeVector(float a[3]); + +// 3D Vector cross product yeilding vector +void crossProductVectors(float c[3], float a[3], float b[3]); + +// initialize matrix +void identityMatrix3x3(float m[3][3]); + +// matrix copy +void copyMatrix3x3(float b[3][3], float a[3][3]); + +// matrix transpose +void transposeMatrix3x3(float b[3][3], float a[3][3]); + +// multiply matrix by scalar +void scaleMatrix3x3(float b[3][3], float s, float a[3][3]); + +// multiply matrix by scalar and add result to another matrix +void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3]); + +// matrix product +// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y] +void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3]); + +// matrix times vector +void matrixDotVector3x3(float p[3], float m[3][3], float v[3]); + +// determinant of matrix +// Computes determinant of matrix m, returning d +void determinant3x3(float * d, float m[3][3]); + +// adjoint of matrix +// Computes adjoint of matrix m, returning a +// (Note that adjoint is just the transpose of the cofactor matrix); +void adjoint3x3(float a[3][3], float m[3][3]); + +// compute adjoint of matrix and scale +// Computes adjoint of matrix m, scales it by s, returning a +void scaleAdjoint3x3(float a[3][3], float s, float m[3][3]); + +// inverse of matrix +// Compute inverse of matrix a, returning determinant m and +// inverse b +void invert3x3(float b[3][3], float a[3][3]); + +// skew matrix from vector +void skew(float a[3][3], float v[3]); + +void printMatrix3X3(float mmm[3][3]); + +void vecPrint(float a[3]); diff --git a/src/AltEst/altitude.cpp b/src/AltEst/altitude.cpp new file mode 100644 index 0000000..8838b36 --- /dev/null +++ b/src/AltEst/altitude.cpp @@ -0,0 +1,58 @@ +/* + altitude.cpp: Altitude estimation via barometer/accelerometer fusion +*/ + +#include "filters.h" +#include "algebra.h" +#include "altitude.h" + +AltitudeEstimator::AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro, + float ca, float accelThreshold) +:kalman(ca, sigmaGyro, sigmaAccel), complementary(sigmaAccel, sigmaBaro, accelThreshold) +{ + this->sigmaAccel = sigmaAccel; + this->sigmaGyro = sigmaGyro; + this->sigmaBaro = sigmaBaro; + this->ca = ca; + this->accelThreshold = accelThreshold; +} + +void AltitudeEstimator::estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp) +{ + float deltat = (float)(timestamp-previousTime)/1000000.0f; + float verticalAccel = kalman.estimate(pastGyro, + pastAccel, + deltat); + complementary.estimate(& estimatedVelocity, + & estimatedAltitude, + baroHeight, + pastAltitude, + pastVerticalVelocity, + pastVerticalAccel, + deltat); + // update values for next iteration + copyVector(pastGyro, gyro); + copyVector(pastAccel, accel); + pastAltitude = estimatedAltitude; + pastVerticalVelocity = estimatedVelocity; + pastVerticalAccel = verticalAccel; + previousTime = timestamp; +} + +float AltitudeEstimator::getAltitude() +{ + // return the last estimated altitude + return estimatedAltitude; +} + +float AltitudeEstimator::getVerticalVelocity() +{ + // return the last estimated vertical velocity + return estimatedVelocity; +} + +float AltitudeEstimator::getVerticalAcceleration() +{ + // return the last estimated vertical acceleration + return pastVerticalAccel; +} diff --git a/src/AltEst/altitude.h b/src/AltEst/altitude.h new file mode 100644 index 0000000..1ca6cb0 --- /dev/null +++ b/src/AltEst/altitude.h @@ -0,0 +1,55 @@ +/* + altitude.h: Altitude estimation via barometer/accelerometer fusion +*/ + +# pragma once + +#include "filters.h" +#include "algebra.h" +#include "pico/time.h" +#include "pico/types.h" + +class AltitudeEstimator { + + private: + // required parameters for the filters used for the estimations + // sensor's standard deviations + float sigmaAccel; + float sigmaGyro; + float sigmaBaro; + // Acceleration markov chain model state transition constant + float ca; + // Zero-velocity update acceleration threshold + float accelThreshold; + // gravity + float g = 9.81; + // For computing the sampling period + absolute_time_t prevTime = get_absolute_time(); + uint32_t previousTime = to_us_since_boot(prevTime); + // required filters for altitude and vertical velocity estimation + KalmanFilter kalman; + ComplementaryFilter complementary; + // Estimated past vertical acceleration + float pastVerticalAccel = 0; + float pastVerticalVelocity = 0; + float pastAltitude = 0; + float pastGyro[3] = {0, 0, 0}; + float pastAccel[3] = {0, 0, 0}; + // estimated altitude and vertical velocity + float estimatedAltitude = 0; + float estimatedVelocity = 0; + + public: + + AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro, + float ca, float accelThreshold); + + void estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp); + + float getAltitude(); + + float getVerticalVelocity(); + + float getVerticalAcceleration(); + +}; // class AltitudeEstimator diff --git a/src/AltEst/filters.cpp b/src/AltEst/filters.cpp new file mode 100644 index 0000000..7902065 --- /dev/null +++ b/src/AltEst/filters.cpp @@ -0,0 +1,202 @@ +/* + filters.cpp: Filter class implementations + */ + +//#include <cmath> +#include <stdlib.h> // XXX eventually use fabs() instead of abs() ? + +#include "filters.h" + +void KalmanFilter::getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat) +{ + // required matrices for the operations + float sigma[3][3]; + float identity[3][3]; + identityMatrix3x3(identity); + float skewMatrix[3][3]; + skew(skewMatrix, previousState); + float tmp[3][3]; + // Compute the prediction covariance matrix + scaleMatrix3x3(sigma, pow(sigmaGyro, 2), identity); + matrixProduct3x3(tmp, skewMatrix, sigma); + matrixProduct3x3(covariance, tmp, skewMatrix); + scaleMatrix3x3(covariance, -pow(deltat, 2), covariance); +} + +void KalmanFilter::getMeasurementCovariance(float covariance[3][3]) +{ + // required matrices for the operations + float sigma[3][3]; + float identity[3][3]; + identityMatrix3x3(identity); + float norm; + // Compute measurement covariance + scaleMatrix3x3(sigma, pow(sigmaAccel, 2), identity); + vectorLength(& norm, previousAccelSensor); + scaleAndAccumulateMatrix3x3(sigma, (1.0/3.0)*pow(ca, 2)*norm, identity); + copyMatrix3x3(covariance, sigma); +} + +void KalmanFilter::predictState(float predictedState[3], float gyro[3], float deltat) +{ + // helper matrices + float identity[3][3]; + identityMatrix3x3(identity); + float skewFromGyro[3][3]; + skew(skewFromGyro, gyro); + // Predict state + scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro); + matrixDotVector3x3(predictedState, identity, currentState); + normalizeVector(predictedState); +} + +void KalmanFilter::predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat) +{ + // required matrices + float Q[3][3]; + float identity[3][3]; + identityMatrix3x3(identity); + float skewFromGyro[3][3]; + skew(skewFromGyro, gyro); + float tmp[3][3]; + float tmpTransposed[3][3]; + float tmp2[3][3]; + // predict error covariance + getPredictionCovariance(Q, currentState, deltat); + scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro); + copyMatrix3x3(tmp, identity); + transposeMatrix3x3(tmpTransposed, tmp); + matrixProduct3x3(tmp2, tmp, currErrorCovariance); + matrixProduct3x3(covariance, tmp2, tmpTransposed); + scaleAndAccumulateMatrix3x3(covariance, 1.0, Q); +} + +void KalmanFilter::updateGain(float gain[3][3], float errorCovariance[3][3]) +{ + // required matrices + float R[3][3]; + float HTransposed[3][3]; + transposeMatrix3x3(HTransposed, H); + float tmp[3][3]; + float tmp2[3][3]; + float tmp2Inverse[3][3]; + // update kalman gain + // P.dot(H.T).dot(inv(H.dot(P).dot(H.T) + R)) + getMeasurementCovariance(R); + matrixProduct3x3(tmp, errorCovariance, HTransposed); + matrixProduct3x3(tmp2, H, tmp); + scaleAndAccumulateMatrix3x3(tmp2, 1.0, R); + invert3x3(tmp2Inverse, tmp2); + matrixProduct3x3(gain, tmp, tmp2Inverse); +} + +void KalmanFilter::updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3]) +{ + // required matrices + float tmp[3]; + float tmp2[3]; + float measurement[3]; + scaleVector(tmp, ca, previousAccelSensor); + subtractVectors(measurement, accel, tmp); + // update state with measurement + // predicted_state + K.dot(measurement - H.dot(predicted_state)) + matrixDotVector3x3(tmp, H, predictedState); + subtractVectors(tmp, measurement, tmp); + matrixDotVector3x3(tmp2, gain, tmp); + sumVectors(updatedState, predictedState, tmp2); + normalizeVector(updatedState); +} + +void KalmanFilter::updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3]) +{ + // required matrices + float identity[3][3]; + identityMatrix3x3(identity); + float tmp[3][3]; + float tmp2[3][3]; + // update error covariance with measurement + matrixProduct3x3(tmp, gain, H); + matrixProduct3x3(tmp2, tmp, errorCovariance); + scaleAndAccumulateMatrix3x3(identity, -1.0, tmp2); + copyMatrix3x3(covariance, tmp2); +} + + +KalmanFilter::KalmanFilter(float ca, float sigmaGyro, float sigmaAccel) +{ + this->ca = ca; + this->sigmaGyro = sigmaGyro; + this->sigmaAccel = sigmaAccel; +} + +float KalmanFilter::estimate(float gyro[3], float accel[3], float deltat) +{ + float predictedState[3]; + float updatedState[3]; + float errorCovariance[3][3]; + float updatedErrorCovariance[3][3]; + float gain[3][3]; + float accelSensor[3]; + float tmp[3]; + float accelEarth; + scaleVector(accel, 9.81, accel); // Scale accel readings since they are measured in gs + // perform estimation + // predictions + predictState(predictedState, gyro, deltat); + predictErrorCovariance(errorCovariance, gyro, deltat); + // updates + updateGain(gain, errorCovariance); + updateState(updatedState, predictedState, gain, accel); + updateErrorCovariance(updatedErrorCovariance, errorCovariance, gain); + // Store required values for next iteration + copyVector(currentState, updatedState); + copyMatrix3x3(currErrorCovariance, updatedErrorCovariance); + // return vertical acceleration estimate + scaleVector(tmp, 9.81, updatedState); + subtractVectors(accelSensor, accel, tmp); + copyVector(previousAccelSensor, accelSensor); + dotProductVectors(& accelEarth, accelSensor, updatedState); + return accelEarth; +} + + +float ComplementaryFilter::ApplyZUPT(float accel, float vel) +{ + // first update ZUPT array with latest estimation + ZUPT[ZUPTIdx] = accel; + // and move index to next slot + uint8_t nextIndex = (ZUPTIdx + 1) % ZUPT_SIZE; + ZUPTIdx = nextIndex; + // Apply Zero-velocity update + for (uint8_t k = 0; k < ZUPT_SIZE; ++k) { + if (abs(ZUPT[k]) > accelThreshold) return vel; + } + return 0.0; +} + + +ComplementaryFilter::ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold) +{ + // Compute the filter gain + gain[0] = sqrt(2 * sigmaAccel / sigmaBaro); + gain[1] = sigmaAccel / sigmaBaro; + // If acceleration is below the threshold the ZUPT counter + // will be increased + this->accelThreshold = accelThreshold; + // initialize zero-velocity update + ZUPTIdx = 0; + for (uint8_t k = 0; k < ZUPT_SIZE; ++k) { + ZUPT[k] = 0; + } +} + +void ComplementaryFilter::estimate(float * velocity, float * altitude, float baroAltitude, + float pastAltitude, float pastVelocity, float accel, float deltat) +{ + // Apply complementary filter + *altitude = pastAltitude + deltat*(pastVelocity + (gain[0] + gain[1]*deltat/2)*(baroAltitude-pastAltitude))+ + accel*pow(deltat, 2)/2; + *velocity = pastVelocity + deltat*(gain[1]*(baroAltitude-pastAltitude) + accel); + // Compute zero-velocity update + *velocity = ApplyZUPT(accel, *velocity); +} diff --git a/src/AltEst/filters.h b/src/AltEst/filters.h new file mode 100644 index 0000000..2e316a3 --- /dev/null +++ b/src/AltEst/filters.h @@ -0,0 +1,65 @@ +/* + filters.h: Filter class declarations + */ + +#pragma once + +//#include <cmath> +#include <math.h> +#include <stdint.h> + +#include "algebra.h" + +class KalmanFilter { + private: + float currentState[3] = {0, 0, 1}; + float currErrorCovariance[3][3] = {{100, 0, 0},{0, 100, 0},{0, 0, 100}}; + float H[3][3] = {{9.81, 0, 0}, {0, 9.81, 0}, {0, 0, 9.81}}; + float previousAccelSensor[3] = {0, 0, 0}; + float ca; + float sigmaGyro; + float sigmaAccel; + + void getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat); + + void getMeasurementCovariance(float covariance[3][3]); + + void predictState(float predictedState[3], float gyro[3], float deltat); + + void predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat); + + void updateGain(float gain[3][3], float errorCovariance[3][3]); + + void updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3]); + + void updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3]); + + public: + + KalmanFilter(float ca, float sigmaGyro, float sigmaAccel); + + float estimate(float gyro[3], float accel[3], float deltat); + +}; // Class KalmanFilter + +class ComplementaryFilter { + + private: + + // filter gain + float gain[2]; + // Zero-velocity update + float accelThreshold; + static const uint8_t ZUPT_SIZE = 12; + uint8_t ZUPTIdx; + float ZUPT[ZUPT_SIZE]; + + float ApplyZUPT(float accel, float vel); + + public: + + ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold); + + void estimate(float * velocity, float * altitude, float baroAltitude, + float pastAltitude, float pastVelocity, float accel, float deltat); +}; // Class ComplementaryFilter diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0a50318..59d21de 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,49 +1,54 @@ add_executable(ads - active_drag_system.cpp -) - -add_executable(pru1 - pru1/sensors.cpp -) - -add_executable(pru2 - pru2/servos.cpp -) - -target_link_options(pru1 PRIVATE -static) -target_link_options(pru2 PRIVATE -static) -cmake_minimum_required(VERSION 3.16.3) - -include_directories( ../include ) - -# Set Variables -set(TARGET ActiveDragSystem) -set(SOURCES active-drag-system.cpp ads.cpp actuationPlan.cpp surfaceFitModel.cpp rocketUtils.cpp sensorIMU.cpp sensorAltimeter.cpp motor.cpp logger.cpp kalmanfilter.cpp) - - -# Create Executables & Link Dependencies -add_executable(${TARGET} ${SOURCES}) - -add_executable(${TARGET_B} ${SOURCES_B}) -target_link_libraries(${TARGET_B} PUBLIC gtest_main) -add_test(NAME ${TARGET_B} COMMAND ${TARGET_B}) - -add_executable(${TARGET_C} ${SOURCES_C}) -target_link_libraries(${TARGET_C} PUBLIC gtest_main) -add_test(NAME ${TARGET_C} COMMAND ${TARGET_C}) - -add_executable(${TARGET_D} ${SOURCES_D}) -target_link_libraries(${TARGET_D} PUBLIC gtest_main) -add_test(NAME ${TARGET_D} COMMAND ${TARGET_D}) - -add_executable(${TARGET_E} ${SOURCES_E}) -target_link_libraries(${TARGET_E} PUBLIC gtest_main) -add_test(NAME ${TARGET_E} COMMAND ${TARGET_E}) - -add_executable(${TARGET_F} ${SOURCES_F}) -target_link_libraries(${TARGET_F} PUBLIC gtest_main) -add_test(NAME ${TARGET_F} COMMAND ${TARGET_F}) - -add_executable(${TARGET_G} ${SOURCES_G}) -target_link_libraries(${TARGET_G} PUBLIC gtest_main) -add_test(NAME ${TARGET_G} COMMAND ${TARGET_G}) + active_drag_system.cpp + spi_flash.c + bno055.cpp + SimpleKalmanFilter.cpp + # kalmanfilter.cpp + pwm.cpp + AltEst/algebra.cpp + AltEst/altitude.cpp + AltEst/filters.cpp + ) + +add_executable(read_flash + read_flash.c + spi_flash.c + ) + +add_executable(servo_test + servo_test.cpp + pwm.cpp + ) + +add_executable(alt_test + altimeter.cpp + ) + +# pull in common dependencies +target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm pico_cyw43_arch_none ${Eigen_LIBRARIES}) +target_include_directories(ads PUBLIC ../include) + +target_link_libraries(read_flash pico_stdlib hardware_spi) +target_include_directories(read_flash PUBLIC ../include) + +target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c) +target_include_directories(servo_test PUBLIC ../include) + +target_link_libraries(alt_test pico_stdlib hardware_i2c hardware_gpio) + +pico_enable_stdio_usb(ads 1) +pico_enable_stdio_uart(ads 0) + +pico_enable_stdio_usb(read_flash 1) +pico_enable_stdio_uart(read_flash 0) + +pico_enable_stdio_usb(servo_test 1) +pico_enable_stdio_uart(servo_test 0) + +pico_enable_stdio_usb(alt_test 1) +pico_enable_stdio_uart(alt_test 0) +# create map/bin/hex file etc. +pico_add_extra_outputs(ads) +pico_add_extra_outputs(read_flash) +pico_add_extra_outputs(servo_test) +pico_add_extra_outputs(alt_test) diff --git a/src/SimpleKalmanFilter.cpp b/src/SimpleKalmanFilter.cpp new file mode 100644 index 0000000..88ba5d4 --- /dev/null +++ b/src/SimpleKalmanFilter.cpp @@ -0,0 +1,48 @@ +/* + * SimpleKalmanFilter - a Kalman Filter implementation for single variable models. + * Created by Denys Sene, January, 1, 2017. + * Released under MIT License - see LICENSE file for details. + */ + +#include "SimpleKalmanFilter.h" +#include <math.h> + +SimpleKalmanFilter::SimpleKalmanFilter(float mea_e, float est_e, float q) +{ + _err_measure=mea_e; + _err_estimate=est_e; + _q = q; +} + +float SimpleKalmanFilter::updateEstimate(float mea) +{ + _kalman_gain = _err_estimate/(_err_estimate + _err_measure); + _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate); + _err_estimate = (1.0f - _kalman_gain)*_err_estimate + fabsf(_last_estimate-_current_estimate)*_q; + _last_estimate=_current_estimate; + + return _current_estimate; +} + +void SimpleKalmanFilter::setMeasurementError(float mea_e) +{ + _err_measure=mea_e; +} + +void SimpleKalmanFilter::setEstimateError(float est_e) +{ + _err_estimate=est_e; +} + +void SimpleKalmanFilter::setProcessNoise(float q) +{ + _q=q; +} + +float SimpleKalmanFilter::getKalmanGain() { + return _kalman_gain; +} + +float SimpleKalmanFilter::getEstimateError() { + return _err_estimate; +} diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp index 7b63faa..5d248e2 100644 --- a/src/active_drag_system.cpp +++ b/src/active_drag_system.cpp @@ -1,14 +1,649 @@ -#include <iostream> -#include <cstdlib> -#include "../include/surfaceFitModel.hpp" -#include "../include/actuationPlan.hpp" -#include "../include/ads.hpp" +#include <algorithm> +#include <stdio.h> +#include "pico/multicore.h" +#include "pico/platform.h" +#include "pico/sem.h" +#include "spi_flash.h" +#include "boards/pico_w.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include "pico/time.h" +#include "pico/types.h" +#include "pico/cyw43_arch.h" +#include <math.h> +#include "bno055.hpp" +#include "AltEst/altitude.h" +#include "pwm.hpp" +#include "SimpleKalmanFilter.h" + +#define ALT_ADDR 0x60 +#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 GRAVITY -9.81 +#define LOG_RATE_HZ 4 + +#define MOTOR_BURN_TIME 3900 // Burn time in milliseconds for M2500T + +typedef enum { + PAD, + BOOST, + COAST, + APOGEE, + RECOVERY, + END +} state_t; + +BNO055 bno055; +PWM pwm; +static AltitudeEstimator vKF = AltitudeEstimator(0.0005, // sigma Accel + 0.0005, // sigma Gyro + 0.018, // sigma Baro + 0.5, // ca + 0.1);// accelThreshold + +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); +void init_altimeter(); +float get_deploy_percent(float velocity, float altitude); + +bool timer_callback(repeating_timer_t *rt); +bool test_timer_callback(repeating_timer_t *rt); + +float get_altitude(); +float get_velocity(); + +void snapshot(); +bool logging_callback(repeating_timer_t *rt); +void logging_core(); + +semaphore_t sem; + +volatile float altitude = 0.0f; +volatile float prev_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 vector3f linear_acceleration; +volatile vector3f acceleration; +volatile quarternion abs_quaternion; +volatile vector3f velocity_vector; + +volatile vector3f euler_angles; +volatile vector3f abs_lin_accel; +volatile vector3f prev_abs_lin_accel; +volatile vector3f rot_y_vec; +volatile vector3f vel_at_angle; + +volatile vector3f accel_gravity; + +volatile CALIB_STATUS calib_status; +uint8_t accel[6]; +uint8_t quat[8]; + +repeating_timer_t data_timer; +repeating_timer_t log_timer; + +float ground_altitude = 0.0f; + +SimpleKalmanFilter altitudeKF(2, 2, 0.01); +SimpleKalmanFilter velocityKF(1, 1, 0.01); + +/** + * @brief Main function + * + * @return int error code + */ int main() { + // stdio_init_all(); + + cyw43_arch_init(); + 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); + + // 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(); + + + // Initialize altimeter + init_altimeter(); + + // Initialize BNO055 + bno055.init(); + + // Initialize PWM + pwm.init(); + + // Initialize MOSFET + gpio_init(MOSFET_PIN); + gpio_set_dir(MOSFET_PIN, GPIO_OUT); + + // Initialize Kalman Filter + float measurement = get_altitude(); + float v_measurement = get_velocity(); + altitudeKF.updateEstimate(measurement); + velocityKF.updateEstimate(measurement); + + ground_altitude = altitude; + + sem_init(&sem, 1, 1); + + if (!add_repeating_timer_us(-1000000 / DATA_RATE_HZ, &timer_callback, NULL, &data_timer)) { + // printf("Failed to add timer!\n"); + return -1; + } + + multicore_launch_core1(logging_core); + + while (1) { + tight_loop_contents(); + } +} + +void logging_core() { + add_repeating_timer_us(-1000000 / LOG_RATE_HZ, &logging_callback, NULL, &log_timer); + + while (1) { + tight_loop_contents(); + } +} + +/** + * @brief Initializes the altimeter + * + * @param altitude passes the altitude variable to the function + * @param threshold_altitude passes the threshold altitude variable to the function + */ +void init_altimeter() { + + uint8_t config[2] = {0}; + + // Select control register(0x26) + // Active mode, OSR = 16, altimeter mode(0xB8) + config[0] = 0x26; + config[1] = 0x89; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select data configuration register(0x13) + // Data ready event enabled for altitude, pressure, temperature(0x07) + // config[0] = 0x13; + // config[1] = 0x07; + // i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Below configures the interrupt for the first transition from PAD to BOOST + // Initial Reading + + sleep_ms(1000); + + while (altitude == 0.0f) { + altitude = get_altitude(); + } + + threshold_altitude += altitude; // 30 meters above ground + + // printf("threshold_altitude: %4.2f", threshold_altitude); + + // Select control register 3 (0x28) + // Set bot interrupt pins to active low and enable internal pullups + config[0] = 0x28; + config[1] = 0x01; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select pressure target MSB register(0x16) + // Set altitude target to 30 meters above ground altitude + config[0] = 0x16; + config[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8); + // printf("threshold_alt upper half: %X\n", config[1]); + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select pressure target LSB register(0x17) + // Set altitude target to 30 meters above ground altitude + config[0] = 0x17; + config[1] = (uint8_t) (((int16_t)(threshold_altitude))); + // printf("threshold_alt lower half: %X\n", config[1]); + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + config[0] = 0x29; + config[1] = 0x08; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select interrupt configuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + config[0] = 0x2A; + config[1] = 0x08; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &pad_callback); + // End of configuration of interrupt for first transition from PAD to BOOST +} + +void snapshot() { + if (state != END) { + uint8_t entry[PACKET_SIZE]; + absolute_time_t now = get_absolute_time(); + uint64_t now_us = to_us_since_boot(now); + uint32_t alt_bits = *((uint32_t *)&altitude); + uint32_t vel_bits = *((uint32_t *)&velocity); + uint32_t acc_bits = *((uint32_t *)&abs_lin_accel.z); + 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; + + switch (state) { + case PAD: + entry[8] = 'P'; + break; + case BOOST: + entry[8] = 'B'; + break; + case COAST: + entry[8] = 'C'; + break; + case APOGEE: + entry[8] = 'A'; + break; + case RECOVERY: + entry[8] = 'R'; + break; + case END: + entry[8] = 'E'; + break; + } + + entry[9] = deployment_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] = quat[0]; + entry[19] = quat[1]; + entry[20] = quat[2]; + entry[21] = quat[3]; + entry[22] = quat[4]; + entry[23] = quat[5]; + entry[24] = quat[6]; + entry[25] = quat[7]; + + entry[26] = acc_bits >> 24; + entry[27] = acc_bits >> 16; + entry[28] = acc_bits >> 8; + entry[29] = acc_bits; + entry[30] = accel[4]; + entry[31] = accel[5]; + write_entry(entry); + } +} + +bool logging_callback(repeating_timer_t *rt) { + static bool LED_STATUS = 0; + sem_acquire_blocking(&sem); + LED_STATUS = !LED_STATUS; + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, LED_STATUS); + snapshot(); + sem_release(&sem); + return true; +} + +bool timer_callback(repeating_timer_t *rt) { + absolute_time_t last = get_absolute_time(); + sem_acquire_blocking(&sem); + float measurement = get_altitude(); + altitude = altitudeKF.updateEstimate(measurement); + // float in_velocity = (altitude - prev_altitude) * DATA_RATE_HZ; + // velocity = velocityKF.updateEstimate(in_velocity); + float acceldata[3]; + float gyrodata[3]; + + + // printf("Velocity_Delta: %4.2f\tVelocity_Prev: %4.2f\n", velocity, ((altitude - prev_altitude) * DATA_RATE_HZ)); + + bno055.read_lin_accel(); + // printf("Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n", + // linear_acceleration.x, linear_acceleration.y, linear_acceleration.z); + + bno055.read_abs_quaternion(); + // printf("Absolute Quaternion:\n" "w: %f\n" "x: %f\n" "y: %f\n" "z: %f\n", + // abs_quaternion.w, abs_quaternion.x, abs_quaternion.y, abs_quaternion.z); + + bno055.read_euler_angles(); + // printf("Euler Angles:\n" "Roll: %f\n" "Pitch: %f\n" "Yaw: %f\n", + // euler_angles.x, euler_angles.y, euler_angles.z); + + // Linear Acceleration and Absolute Quaternion are used to calculate Absolute Linear Acceleration + // They must be read before calling this function + bno055.calculate_abs_linear_acceleration(); + // printf("Absolute Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n", + // abs_lin_accel.x, abs_lin_accel.y, abs_lin_accel.z); - SurfaceFitModel sfm = SurfaceFitModel(); - ActuationPlan plan = ActuationPlan(sfm); - ADS ads = ADS(plan); - ads.run(); - return EXIT_SUCCESS; + acceldata[0] = abs_lin_accel.x; + acceldata[1] = abs_lin_accel.y; + acceldata[2] = abs_lin_accel.z - 0.4f; + gyrodata[0] = 0; + gyrodata[1] = 0; + gyrodata[2] = 0; + + vKF.estimate(acceldata, gyrodata, altitude, to_us_since_boot(last)); + velocity = vKF.getVerticalVelocity(); + // printf("Measurement: %4.2f, Altitude: %4.2f, Velocity: %4.2f\n", measurement, altitude, velocity); + // This is wrong but i'm going home. + // velocity_vector.x = (prev_abs_lin_accel.x - abs_lin_accel.x) / 0.01f); + // velocity_vector.y = (prev_abs_lin_accel.y - abs_lin_accel.y) / 0.01f); + // velocity_vector.z = (prev_abs_lin_accel.z - abs_lin_accel.z) / 0.01f); + // printf("Velocity Vector:\n" "x: %f\n" "y: %f\n" "z: %f\n", + // velocity_vector.x, velocity_vector.y, velocity_vector.z); + + prev_abs_lin_accel.x = abs_lin_accel.x; + prev_abs_lin_accel.y = abs_lin_accel.y; + prev_abs_lin_accel.z = abs_lin_accel.z; + + bno055.accel_to_gravity(); + float agl = altitude - ground_altitude; + + deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, agl)), 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; + } + prev_altitude = altitude; + sem_release(&sem); + return true; } + +/** + * @brief Test function for timer callback outputs data in ROS2 format + * + * @param rt + * @return true + * @return false + */ +// bool test_timer_callback(repeating_timer_t *rt) { +// static float prev_altitude = altitude; +// absolute_time_t last = get_absolute_time(); +// altitude = get_altitude(); +// velocity = ((altitude - prev_altitude) / 0.01f); +// prev_altitude = altitude; +// +// bno055.read_lin_accel(); +// bno055.read_abs_quaternion(); +// +// absolute_time_t now = get_absolute_time(); +// int64_t time_delta = absolute_time_diff_us(last, now); +// +// std::cout << altitude << " " << abs_quaternion.w << " " +// << abs_quaternion.x << " " +// << abs_quaternion.y << " " +// << abs_quaternion.z << " " +// << linear_acceleration.x << " " +// << linear_acceleration.y << " " +// << linear_acceleration.z << std::endl; +// +// /* switch (state) { +// case PAD: +// printf("P\n"); +// break; +// case BOOST: +// printf("B\n"); +// break; +// case COAST: +// printf("C\n"); +// break; +// case APOGEE: +// printf("A\n"); +// break; +// case RECOVERY: +// printf("R\n"); +// break; +// case END: +// printf("E\n"); +// break; +// }*/ +// +// // absolute_time_t now = get_absolute_time(); +// // int64_t time_delta = absolute_time_diff_us(last, now); +// // printf("Time Delta: %" PRIi64"\n", time_delta); +// // std::flush(std::cout); +// prev_altitude = altitude; +// return true; +// } + +/** + * @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) { + + /// @link https://www.raspberrypi.com/documentation/pico-sdk/hardware.html#ga6165f07f4b619dd08ea6dc97d069e78a + /// Each pin only supports one call back, so by calling this we overwrite the previous one + gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, false, &pad_callback); + uint8_t config[2] = {0}; + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + config[0] = 0x29; + config[1] = 0x00; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select interrupt configuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + config[0] = 0x2A; + config[1] = 0x00; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + sem_acquire_blocking(&sem); + state = BOOST; + // start motor burn timer with this function as callback + add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false); + snapshot(); + sem_release(&sem); +} + +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; + snapshot(); + sem_release(&sem); + add_alarm_in_ms(1000, &coast_callback, NULL, false); + return 0; +} + +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; + snapshot(); + sem_release(&sem); + add_alarm_in_ms(1, &apogee_callback, NULL, false); + } else { + add_alarm_in_ms(250, &coast_callback, NULL, false); + } + return 0; +} + +int64_t apogee_callback(alarm_id_t id, void* user_data) { + // Set altimeter interrupt to occur for when rocket touches back to the ground + + uint8_t config[2] = {0}; + // Select pressure target MSB register(0x16) + // Set altitude target to 10 meters above ground altitude + float recov_altitude = ground_altitude + 10.0f; + // Select pressure target MSB register(0x16) + // Set altitude target to 30 meters above ground altitude + config[0] = 0x16; + config[1] = (uint8_t) (((int16_t)(recov_altitude)) >> 8); + // printf("threshold_alt upper half: %X\n", config[1]); + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select pressure target LSB register(0x17) + // Set altitude target to 30 meters above ground altitude + config[0] = 0x17; + config[1] = (uint8_t) (((int16_t)(recov_altitude))); + // printf("threshold_alt lower half: %X\n", config[1]); + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select interrupt enable register (0x29) + // Set interrupt enabled for altitude threshold(0x08) + config[0] = 0x29; + config[1] = 0x08; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + // Select interrupt configuration register (0x2A) + // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08) + config[0] = 0x2A; + config[1] = 0x08; + i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true); + + sem_acquire_blocking(&sem); + state = RECOVERY; + snapshot(); + sem_release(&sem); + gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &recovery_callback); + return 0; +} + +void recovery_callback(uint gpio, uint32_t event_mask) { + // Essentially just a signal to stop logging data + sem_acquire_blocking(&sem); + state = END; + snapshot(); + sem_acquire_blocking(&sem); +} + +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; + // uint32_t temp_alt = (data[1] << 24) | (data[2] << 16) | (data[3] << 8); + // float altitude = temp_alt / 65536.0f; + return altitude; +} + +/** + * @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)); + + +} + +float get_velocity() { + uint8_t reg = 0x07; + uint8_t data[5]; + i2c_write_blocking(i2c_default, ALT_ADDR, ®, 1, true); + i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false); + float delta = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625; + float vel = delta * DATA_RATE_HZ; + return vel; +} + diff --git a/src/altimeter.cpp b/src/altimeter.cpp new file mode 100644 index 0000000..a5c8849 --- /dev/null +++ b/src/altimeter.cpp @@ -0,0 +1,50 @@ +#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 +#define MAX_SCL 400000 +#define DATA_RATE_HZ 15 + +float altitude = 0.0f; +float get_altitude(); + +int main() { + stdio_init_all(); + + 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 config[2] = {0}; + + // 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); + + while (1) { + sleep_ms(1000); + altitude = get_altitude(); + printf("Altitude: %4.2f\n", altitude); + } +} + +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; +} + diff --git a/src/bno055.cpp b/src/bno055.cpp new file mode 100644 index 0000000..fb51986 --- /dev/null +++ b/src/bno055.cpp @@ -0,0 +1,201 @@ +#include "bno055.hpp" + +/// @link [Pico BNO055 Example](https://learnembeddedsystems.co.uk/bno005-i2c-example-code) + +BNO055::BNO055() { + bno055_address = BNO055_ADDRESS_A; + _sensorID = BNO055_ID; + default_mode = OPERATION_MODE_NDOF; +} + +void BNO055::reset_bno055() { + uint8_t data[2]; + data[0] = BNO055_SYS_TRIGGER_ADDR; + data[1] = 0x20; // Reset system + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + sleep_ms(1000); // Wait 650ms for the sensor to reset +} + +void BNO055::init() { + sleep_ms(1000); // Wait 650ms for the sensor to reset + uint8_t chip_id_addr = BNO055_CHIP_ID_ADDR; + uint8_t id[1]; + i2c_write_blocking(i2c_default, bno055_address, &chip_id_addr, 1, false); + i2c_read_blocking(i2c_default, bno055_address, id, 1, false); + if (!id[0] == _sensorID) { + printf("BNO055 not detected\n"); + } + + // Use internal oscillator + uint8_t data[2]; + data[0] = BNO055_SYS_TRIGGER_ADDR; + data[1] = 0x40; // Set to use internal oscillator + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + + // Reset all interrupt status bits + data[0] = BNO055_SYS_TRIGGER_ADDR; + data[1] = 0x01; // Reset interrupt status + // 0x05 = Reset system + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + + // Set to normal power mode + data[0] = BNO055_PWR_MODE_ADDR; + data[1] = 0x00; // Normal power mode + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + sleep_ms(50); // Wait 50ms for the sensor to switch to normal power mode + + // Page 25 of the datasheet + // Default Axis Config + data[0] = BNO055_AXIS_MAP_CONFIG_ADDR; + data[1] = 0x24; // P1=Z, P2=Y, P3=X + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + + // Default Axis Sign + data[0] = BNO055_AXIS_MAP_SIGN_ADDR; + data[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + + // Set units to m/s^2 + data[0] = BNO055_UNIT_SEL_ADDR; + data[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2 + i2c_write_blocking(i2c_default, bno055_address, data, 2, true); + sleep_ms(30); + + //The default operation mode after power-on is CONFIGMODE + // Set mode to NDOF + // Takes 7ms to switch from CONFIG mode; see page 21 on datasheet (3.3) + data[0] = BNO055_OPR_MODE_ADDR; + data[1] = default_mode; // NDOF + i2c_write_blocking(i2c_default, bno055_address, data, 2, false); + sleep_ms(100); + + +} + +void BNO055::read_calib_status() { + uint8_t calib_stat_reg = BNO055_CALIB_STAT_ADDR; + uint8_t calib_stat[1]; + i2c_write_blocking(i2c_default, bno055_address, &calib_stat_reg, 1, true); + i2c_read_blocking(i2c_default, bno055_address, calib_stat, 1, false); + calib_status.mag = ((calib_stat[0] & 0b00000011) >> 0); + calib_status.accel = ((calib_stat[0] & 0b00001100) >> 2); + calib_status.gyro = ((calib_stat[0] & 0b00110000) >> 4); + calib_status.sys = ((calib_stat[0] & 0b11000000) >> 6); +} + +void BNO055::read_lin_accel() { + 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, accel, 6, false); + int16_t x, y, z; + x = y = z = 0; + x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8); + y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8); + z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8); + linear_acceleration.x = ((float)x) / 100.0; + linear_acceleration.y = ((float)y) / 100.0; + linear_acceleration.z = ((float)z) / 100.0; +} + +void BNO055::clamp_close_zero(volatile float &val) { + if (val < 0.01 && val > -0.01) { + val = 0.0; + } +} + +void BNO055::accel_to_gravity() { + accel_gravity.x = abs_lin_accel.x / 9.81; + accel_gravity.y = abs_lin_accel.y / 9.81; + accel_gravity.z = abs_lin_accel.z / 9.81; +} + +void BNO055::read_abs_quaternion() { + 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, x, y, z; + 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; +} + +void BNO055::read_euler_angles() { + uint8_t euler[6]; + uint8_t euler_reg = BNO055_EULER_H_LSB_ADDR; + i2c_write_blocking(i2c_default, bno055_address, &euler_reg, 1, true); + i2c_read_blocking(i2c_default, bno055_address, euler, 6, false); + /// @note heading = yaw + int16_t heading, roll, pitch; + heading = roll = pitch = 0; + heading = ((int16_t)euler[0]) | (((int16_t)euler[1]) << 8); + roll = ((int16_t)euler[2]) | (((int16_t)euler[3]) << 8); + pitch = ((int16_t)euler[4]) | (((int16_t)euler[5]) << 8); + euler_angles.x = ((float)roll) / 16.0; + euler_angles.y = ((float)pitch) / 16.0; + euler_angles.z = ((float)heading) / 16.0; +} + +void BNO055::read_accel() { + uint8_t accel[6]; + uint8_t accel_reg = BNO055_ACCEL_DATA_X_LSB_ADDR; + i2c_write_blocking(i2c_default, bno055_address, &accel_reg, 1, true); + i2c_read_blocking(i2c_default, bno055_address, accel, 6, false); + int16_t x, y, z; + x = y = z = 0; + x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8); + y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8); + z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8); + acceleration.x = ((float)x) / 100.0; + acceleration.y = ((float)y) / 100.0; + acceleration.z = ((float)z) / 100.0; +} + +void BNO055::quaternion_to_euler() { + 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 m = q.toRotationMatrix(); + euler_angles.x = atan2f(m(2,1), m(2,2)); + euler_angles.y = asinf(-m(2,0)); + euler_angles.z = atan2f(m(1,0), m(0,0)); +} + +void BNO055::calculate_abs_linear_acceleration() { + 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; + lin_accel.x() = linear_acceleration.x; + lin_accel.y() = linear_acceleration.y; + lin_accel.z() = linear_acceleration.z; + abs_lin_accel.x = lin_accel.x() * rotation_matrix(0, 0) + lin_accel.y() * rotation_matrix(0, 1) + lin_accel.z() * rotation_matrix(0, 2); + abs_lin_accel.y = lin_accel.x() * rotation_matrix(1, 0) + lin_accel.y() * rotation_matrix(1, 1) + lin_accel.z() * rotation_matrix(1, 2); + abs_lin_accel.z = -1.0f * (lin_accel.x() * rotation_matrix(2, 0) + lin_accel.y() * rotation_matrix(2, 1) + lin_accel.z() * rotation_matrix(2, 2)); +} + +void BNO055::get_rotation_vector() { + 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(); + rot_y_vec.x = rotation_matrix(1, 0); + rot_y_vec.y = rotation_matrix(1, 1); + rot_y_vec.z = rotation_matrix(1, 2); +} + diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp index 735e8c8..8174513 100644 --- a/src/kalmanfilter.cpp +++ b/src/kalmanfilter.cpp @@ -1,3 +1,4 @@ +#include <climits> #include "../include/kalmanfilter.hpp" // Private---------------------------------------------------------------------- @@ -25,7 +26,7 @@ void KalmanFilter::matrixInit() { process_noise_covariance(1,1) = 0.1; // Setup Measurement Covariance - measurement_covariance << 0.1; + measurement_covariance << 1e-12; } diff --git a/src/pwm.cpp b/src/pwm.cpp new file mode 100644 index 0000000..3a78d23 --- /dev/null +++ b/src/pwm.cpp @@ -0,0 +1,35 @@ +#include "pwm.hpp" + +void PWM::init() { + // Tell GPIO 0 they are allocated to the PWM + gpio_set_function(SERVO_PIN, 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); + + // Configure PWM slice and set it running + pwm_config cfg = pwm_get_default_config(); + // Set the PWM clock divider to 38.15 + pwm_config_set_clkdiv(&cfg, CLOCK_DIV_RATE); + // Set the PWM wrap value to 65535 + pwm_set_wrap(slice_num, WRAP_VALUE); + // Set the PWM duty cycle to 0 and enable the PWM + pwm_init(slice_num, &cfg, true); + + // Enable the PWM again cause idk + pwm_set_enabled(slice_num, true); +} + +void PWM::set_duty_cycle(int duty_cycle_percent) { + // Calculate the raw value + uint32_t raw_value = WRAP_VALUE * (duty_cycle_percent / 100.0); + + // Set the duty cycle + pwm_set_gpio_level(SERVO_PIN, raw_value); +} + +void PWM::set_servo_percent(int percent) { + // Calculate the value by clamping the percent from 0 to 100 + // to the SERVO_MIN and SERVO_MAX + uint32_t value = ((percent * SERVO_RANGE) / 100) + SERVO_MIN; + PWM::set_duty_cycle(value); +} diff --git a/src/read_flash.c b/src/read_flash.c new file mode 100644 index 0000000..71d2870 --- /dev/null +++ b/src/read_flash.c @@ -0,0 +1,71 @@ +#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 (us) | state | dep pcnt | alt (m) | vel (m/s) | quat_w | quat_x | quat_y | quat_z | lin_ax | lin_ay | lin_az\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); + + int16_t w = ((int16_t)entry[18]) | (((int16_t)entry[19]) << 8); + int16_t x = ((int16_t)entry[20]) | (((int16_t)entry[21]) << 8); + int16_t y = ((int16_t)entry[22]) | (((int16_t)entry[23]) << 8); + int16_t z = ((int16_t)entry[24]) | (((int16_t)entry[25]) << 8); + float qw = ((float)w) / 16384.0; // 2^14 LSB + float qx = ((float)x) / 16384.0; + float qy = ((float)y) / 16384.0; + float qz = ((float)z) / 16384.0; + int16_t ax = ((int16_t)entry[26]) | (((int16_t)entry[27]) << 8); + int16_t ay = ((int16_t)entry[28]) | (((int16_t)entry[29]) << 8); + int16_t az = ((int16_t)entry[30]) | (((int16_t)entry[31]) << 8); + float lax = ((float)x) / 100.0; + float lay = ((float)y) / 100.0; + float laz = ((float)z) / 100.0; + printf("%"PRIu64" | %c | %"PRIu8" | %4.2f | %4.2f | %4.2f | %4.2f| %4.2f | %4.2f | %4.2f | %4.2f |%4.2f\n", \ + now_us, state, deploy_percent, altitude, velocity, qw, qx, qy, qz, lax, lay, laz); + } +} diff --git a/src/servo_test.cpp b/src/servo_test.cpp new file mode 100644 index 0000000..c5e8e6e --- /dev/null +++ b/src/servo_test.cpp @@ -0,0 +1,27 @@ +#include <stdio.h> +#include <inttypes.h> +#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); + pwm.init(); + uint8_t duty_cycle = 13; + while (1) { + getchar(); + if (duty_cycle == 2) { + duty_cycle = 13; + } + pwm.set_duty_cycle(duty_cycle); + printf("Currenty Duty Cycle: %" PRIu8 "\n", duty_cycle); + duty_cycle--; + } +} diff --git a/src/spi_flash.c b/src/spi_flash.c new file mode 100644 index 0000000..32babc5 --- /dev/null +++ b/src/spi_flash.c @@ -0,0 +1,267 @@ +/** + * 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 diff --git a/src/unused/actuationPlan.cpp b/src/unused/actuationPlan.cpp new file mode 100644 index 0000000..a987478 --- /dev/null +++ b/src/unused/actuationPlan.cpp @@ -0,0 +1,60 @@ +#include "../include/actuationPlan.hpp" + +ActuationPlan::ActuationPlan() {} + +ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) { + +} + + +void ActuationPlan::runPlan(Vehicle& rocket) { + + + if (rocket.imuReadFail || rocket.altiReadFail) { + rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment + } + + rocket.fail_time = time(nullptr); + + // 2024 Mission--------------------------------------------------------------------- + if (rocket.status == GLIDE) { + + // Fin deployment based on current drag coefficient value + try { + double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude); + cd = std::min(std::max(0.0, cd), 100.0); + rocket.deployment_angle = deploy_percentage_to_angle(cd); + } + + // Full deployment during coasting + catch (...) { + rocket.deployment_angle = deploy_percentage_to_angle(0); + + if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) { + rocket.deployment_angle = deploy_percentage_to_angle(100); + } + } + } + + else if (rocket.status == APOGEE) { + + rocket.deployment_angle = deploy_percentage_to_angle(50); + } + + else { + + rocket.deploy_time = time(nullptr); + } + // End 2024 Mission------------------------------------------------------------------ +} + + + + + + + + + + + diff --git a/src/unused/ads.cpp b/src/unused/ads.cpp new file mode 100644 index 0000000..5484970 --- /dev/null +++ b/src/unused/ads.cpp @@ -0,0 +1,286 @@ +#include "../include/ads.hpp" + + +// Private---------------------------------------------------------------------- +void ADS::logSummary() { + + std::string output_string = "" + state_for_log[rocket.status]; + + if (!rocket.altiInitFail && !rocket.altiReadFail) { + + output_string += format_data(" ", rocket.filtered_altitude, 3); + } + + output_string += format_data(" ", rocket.deployment_angle, 2); + + if (!rocket.imuInitFail && !rocket.imuReadFail) { + + output_string += format_data(" ", rocket.acceleration[2], 2); + output_string += format_data(" ", rocket.filtered_velocity, 2); + } + + Logger::Get().log(output_string); +} + + +void ADS::updateOnPadAltitude() { + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + double avg_alt = 0; + double alt_read_count = 0; + + while (alt_read_count < COUNT_LIMIT) { + + altimeter.getData(&rocket.current_altitude); + alt_read_count++; + avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count; + } + + Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3)); + rocket.ON_PAD_altitude = avg_alt; +} + + +void ADS::updateSensorData() { + + if (!rocket.imuInitFail) { + + try { + imu.getData((void*)&rocket); + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.imuReadFail = true; + } + } + + rocket.previous_altitude = rocket.current_altitude; // Why was this placed here???? + + if (!rocket.altiInitFail) { + + try { + altimeter.getData((void*)&rocket.current_altitude); + if (rocket.ON_PAD_fail) { + rocket.ON_PAD_altitude = rocket.current_altitude; + rocket.ON_PAD_fail = false; + } + + rocket.altiReadFail = false; + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.altiReadFail = true; + } + } +} + + +void ADS::updateRocketState() { + + // Filter sensor data + VectorXf control_input(1); + VectorXf measurement(1); + control_input << rocket.acceleration[2]; + measurement << rocket.current_altitude; + VectorXf filtered = kf.run(control_input, measurement, rocket.dt); + rocket.filtered_altitude = filtered(0); + rocket.filtered_velocity = filtered(1); + + if (rocket.apogee_altitude < rocket.filtered_altitude) { + rocket.apogee_altitude = rocket.filtered_altitude; + } + + // (VEHICLE ON PAD) + if (rocket.status == ON_PAD) { + + // If launch detected + if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0 + && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) { + Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3)); + } + + if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) { + Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3)); + } + + if (time(nullptr) - rocket.relog_time > 2*60*60 + && rocket.status == ON_PAD) { + std::cout << "OverWR Success" << std::endl; + } + } + + // (VEHICLE BOOSTING) + else if (rocket.status == BOOST) { + + if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0 + || time(nullptr) - rocket.liftoff_time >= TIME_BO) { + rocket.status = GLIDE; + } + + } + + // (VEHICLE IN GLIDE) + else if (rocket.status == GLIDE) { + + if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE + || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) { + rocket.status = APOGEE; + Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2)); + } + } + + // (VEHICLE AT APOGEE) + else if (rocket.status == APOGEE) { + + if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) { + rocket.status = DONE; + return; + } + } +} + + +// Public---------------------------------------------------------------------- +ADS::ADS(ActuationPlan plan) : plan(plan) { + + rocket.status = ON_PAD; + + rocket.apogee_altitude = 0; + rocket.previous_altitude = 0; + rocket.current_altitude = 0; + rocket.filtered_altitude = 0; + + rocket.filtered_velocity = 0; + + rocket.duty_span = DUTY_MAX - DUTY_MIN; + rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); + + rocket.dt = 0.1; + + rocket.imuInitFail = false; + rocket.imuReadFail = false; + rocket.altiInitFail = false; + rocket.altiReadFail = false; + + rocket.ON_PAD_altitude = 0; + rocket.ON_PAD_fail = false; + + rocket.start_time = time(nullptr); + rocket.fail_time = rocket.start_time; + rocket.relog_time = rocket.start_time; + rocket.led_time = rocket.start_time; + + imu = IMUSensor(); + altimeter = AltimeterSensor(); + motor = Motor(); + kf = KalmanFilter(2, 1, 1, rocket.dt); + + Logger::Get().openLog(LOG_FILENAME); + + motor.init(&rocket); + + imu.init(nullptr); + altimeter.init(nullptr); + + if (TEST_MODE) { + + Logger::Get().log("TEST Record Start --"); + } +} + + + +void ADS::run() { + + if (!rocket.altiInitFail) { + try { + updateOnPadAltitude(); + } + + catch (...) { + std::exception_ptr e = std::current_exception(); + Logger::Get().logErr(e.__cxa_exception_type()->name()); + rocket.ON_PAD_fail = true; + } + } + + rocket.loop_time = time(nullptr); + while (rocket.status != DONE) { + + updateSensorData(); + + if (!rocket.imuInitFail && !rocket.altiInitFail) { + + updateRocketState(); + + // Run the Actuation Plan---------------------------------- + plan.runPlan(rocket); + + if (rocket.imuReadFail || rocket.altiReadFail) { + + if (rocket.imuReadFail) { + imu.init(nullptr); // Restart + Logger::Get().log("Altimeter reset attempt"); + } + + if (rocket.altiReadFail) { + altimeter.init(nullptr); // Restart + Logger::Get().log("IMU reset attempt"); + } + } + } + + // Altimeter or IMU setup failed. Attempt to reinitialize + else { + + if (time(nullptr) - rocket.fail_time >= TIME_END) { + rocket.status = DONE; + } + + if (rocket.altiInitFail || rocket.altiReadFail) { + imu.init(nullptr); // Restart + Logger::Get().log("Altimeter reset attempt"); + } + + if (rocket.imuInitFail || rocket.imuReadFail) { + altimeter.init(nullptr); // Restart + Logger::Get().log("IMU reset attempt"); + } + + rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); + } + + // Actuate Servos + motor.writeData(&rocket); + + logSummary(); + + // Blink Beaglebone LED 1 + if (time(nullptr) - rocket.led_time > LED_GAP_TIME) { + led_out(&rocket); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + rocket.dt = time(nullptr) - rocket.loop_time; + rocket.loop_time = time(nullptr); + } + + Logger::Get().closeLog(); + std::cout << "Done" << std::endl; +} + + + + + + + + + + + diff --git a/src/unused/logger.cpp b/src/unused/logger.cpp new file mode 100644 index 0000000..a857be8 --- /dev/null +++ b/src/unused/logger.cpp @@ -0,0 +1,132 @@ +#include "../include/logger.hpp" + +// Private---------------------------------------------------------------------- +std::string Logger::getDate() { + + t = time(nullptr); + now = localtime(&t); + return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " + + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")"; +} + +std::string Logger::getTime() { + + t = time(nullptr); + now = localtime(&t); + std::string hour = std::to_string(now->tm_hour); + std::string min = std::to_string(now->tm_min); + std::string sec = std::to_string(now->tm_sec); + //string hour = "0" + to_string(now->tm_hour); + + if (now->tm_hour < 10) { + hour = "0" + std::to_string(now->tm_hour); + } + + if (now->tm_min < 10) { + min = "0" + std::to_string(now->tm_min); + } + + if (now->tm_sec < 10) { + sec = "0" + std::to_string(now->tm_sec); + } + + return hour + ":" + min + + ":" + sec; +} + + +// Public---------------------------------------------------------------------- +Logger& Logger::Get() { + + static Logger loggerSingleton; + return loggerSingleton; +} + +//Logger Logger::loggerSingleton; + + +bool Logger::openLog(std::string _filename) { + + filename = _filename; + + if (file_open) { + return false; + } + + file.open(filename, std::ios::in | std::ios::out | std::ios::app); + + if (!file) { + return false; + } + + file_open = true; + std::string date = getDate(); + std::string timestamp = getTime(); + file << timestamp << infoTag << "Log Start---- " << date << std::endl; + + return true; +} + + +void Logger::closeLog() { + + std::string timestamp = getTime(); + file << timestamp << infoTag << "Log End----\n\n"; + + file.close(); + file_open = false; +} + + +bool Logger::log(std::string data) { + + if (!file) { + return false; + } + + if (!file_open) { + return false; + } + std::string timestamp = getTime(); + file << timestamp << infoTag << data << std::endl; + return true; +} + +bool Logger::logErr(std::string data) { + + if (!file) { + return false; + } + + if (!file_open) { + return false; + } + + std::string timestamp = getTime(); + file << timestamp << errorTag << data << std::endl; + return true; +} + + +bool Logger::printLog() { + + if (file.is_open()) { + std::cout << "Log still open. Please close Log." << std::endl; + return false; + } + + file.open(filename, std::ios::in); + + if (!file.is_open()) { + return false; + } + + std::string line; + while(getline(file, line)) { + std::cout << line << std::endl; + } + + file.close(); + + return true; +} diff --git a/src/unused/motor.cpp b/src/unused/motor.cpp new file mode 100644 index 0000000..84785a9 --- /dev/null +++ b/src/unused/motor.cpp @@ -0,0 +1,46 @@ +#include "../include/motor.hpp" + + + +Motor::Motor() { + + +} + +bool Motor::init(void* data) { + + Vehicle *vehicle = (Vehicle *) data; + double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN); + + // Initialize stuff + // ..... + // ..... + + + data = (void*) vehicle; // Is this necessary? + return true; +} + + +bool Motor::writeData(void* data) { + + Vehicle *vehicle = (Vehicle *) data; + double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN); + + // Send the Data somewhere + // ..... Pin + // ..... Duty + // ..... PWM frequency Hz + // ..... Polarity + + + if (1 == 2) { + Logger::Get().logErr("Some type of Error"); + return false; + } + + data = (void*) vehicle; // Is this necessary? + return true; +} + + diff --git a/src/unused/rocketUtils.cpp b/src/unused/rocketUtils.cpp new file mode 100644 index 0000000..45fcfc3 --- /dev/null +++ b/src/unused/rocketUtils.cpp @@ -0,0 +1,35 @@ +#include "../include/rocketUtils.hpp" + +double deploy_percentage_to_angle(double percentage) { + + return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE; +} + + +std::string format_data(std::string prefix, double data, int precision) { + + std::stringstream stream; + stream << std::fixed << std::setprecision(precision) << data; + std::string s = stream.str(); + return prefix + s; +} + +bool led_out(Vehicle *vehicle) { + + std::ofstream file; + file.open(LED_FILENAME); + if (!file.is_open()) { + return false; + } + + file << std::to_string(vehicle->led_brightness); + file.close(); + + vehicle->led_time = time(nullptr); + vehicle->led_brightness = (vehicle->led_brightness + 1) % 2; + + return true; +} + +std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"}; +
\ No newline at end of file diff --git a/src/unused/sensorAltimeter.cpp b/src/unused/sensorAltimeter.cpp new file mode 100644 index 0000000..8ec065d --- /dev/null +++ b/src/unused/sensorAltimeter.cpp @@ -0,0 +1,115 @@ +#include "sensorAltimeter.hpp" + +AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) { + I2C_FILE = I2C_FILE_in; + deviceAddress = 0x60; +} + +//Startup routine copied from Adafruit library, as is most of the data getting methods +//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation) +bool AltimeterSensor::init() { + + // Vehicle *vehicle = (Vehicle *) data; + // // Do Stuff + // data = (void*) vehicle; + + //Pass file string from parent to setup function, actual I2C bus gets stored internally. + setupI2C(I2C_FILE); + + // Check a register with a hard-coded value to see if comms are working + uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI); + if (whoami != 0xC4) { + fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami); + return false; + } + + //Send device dedicated reset byte to CTRL1 Register + writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST); + //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register + while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST); + + //Set oversampling (?) and altitude mode by default + currentMode = MPL3115A2_ALTIMETER; + ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + + //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess + writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE | + MPL3115A2_PT_DATA_CFG_PDEFE | + MPL3115A2_PT_DATA_CFG_DREM); + + return true; +} + +//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION +void AltimeterSensor::setMode(mpl3115a2_mode_t mode) { + ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); + ctrl_reg1.bit.ALT = mode; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + currentMode = mode; +} + +double AltimeterSensor::getAltitude() { + //Request new data reading + requestOneShotReading(); + //If new data is available, read it and store it to internal fields + if (isNewDataAvailable()) { + //Logger flag here for new data? + updateCurrentDataBuffer(); + } + //Return internal field, whether updated or not + return internalAltitude; +} + +double AltimeterSensor::getTemperature() { + //Request new data reading + requestOneShotReading(); + //If new data is available, read it and store it to internal fields + if (isNewDataAvailable()) { + //Logger flag here for new data? + updateCurrentDataBuffer(); + } + //Return internal field, whether updated or not + return internalTemperature; +} + +void AltimeterSensor::requestOneShotReading() { + //Request current status of oneshot reading + ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1); + //If oneshot is complete, proc a new one; if it isn't, do nothing. + //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR. + if (!ctrl_reg1.bit.OST) { + // initiate one-shot measurement + ctrl_reg1.bit.OST = 1; + writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg); + } +} + +bool AltimeterSensor::isNewDataAvailable() { + //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available + //There *are* registers available for exclusively temperature *or* pressure/altitude, but + //for simplicity's sake we'll use the combined one for now. + return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0); +} + +//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead +void AltimeterSensor::updateCurrentDataBuffer() { + uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0}; + readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5); + + //Pressure is no longer used, assumed rocket is only logging altitude + // uint32_t pressure; + // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 | + // uint32_t(buffer[2]); + // return double(pressure) / 6400.0; + + //Altitude Conversion + int32_t alt; + alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | + uint32_t(buffer[2]) << 8; + internalAltitude = double(alt) / 65536.0; + + int16_t t; + t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]); + internalTemperature = double(t) / 256.0; +} diff --git a/src/unused/sensorIMU.cpp b/src/unused/sensorIMU.cpp new file mode 100644 index 0000000..941ea35 --- /dev/null +++ b/src/unused/sensorIMU.cpp @@ -0,0 +1,385 @@ +#include "../include/sensorIMU.hpp" + +IMUSensor::IMUSensor(std::string I2C_FILE) { + this -> I2C_FILE = I2C_FILE; +} + +bool IMUSensor::init(void* data) { + + //I2C_File passed on object creation, stored in sensorI2C parent + setupI2C(I2C_FILE); + + //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second + //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that + + //Sanity check for factory device ID + uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR); + if (id != BNO055_ID) { + fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!"); + return false; + } + + //Set default operating mode of IMU into config from startup (will be set properly after config phase) + setModeHard(OPERATION_MODE_CONFIG); + + //Writes 1 to the system reset bit in the trigger register + writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20); + //Wait for reset to complete by doing sanity check again + while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID); + + //Set power mode for sensor + writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); + + //Sensor chip uses two "pages" to multiplex register values + //Page 0 contains the sensor data (not configuration), which is what we want + writeRegister(BNO055_PAGE_ID_ADDR, 0); + + //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess + writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0); + + setModeTemp(default_mode); + + return true; +} + +//Sets mode so it can be undone for temporary changes, like operation setting +void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) { + currentMode = mode; + writeRegister(BNO055_OPR_MODE_ADDR, currentMode); +} + +//Sets mode *AND* internal state variable +void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) { + writeRegister(BNO055_OPR_MODE_ADDR, currentMode); +} + +adafruit_bno055_opmode_t IMUSensor::getMode() { + return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR); +} + +imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) { + imu::Vector<3> xyz; + uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6); + + int16_t x, y, z; + x = y = z = 0; + + /* Read vector data (6 bytes) */ + x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8); + y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8); + z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8); + + /*! + * Convert the value to an appropriate range (section 3.6.4) + * and assign the value to the Vector type + */ + switch (vector_type) { + case VECTOR_MAGNETOMETER: + /* 1uT = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_GYROSCOPE: + /* 1dps = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_EULER: + /* 1 degree = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_ACCELEROMETER: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_LINEARACCEL: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_GRAVITY: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + } + + return xyz; +} +imu::Quaternion IMUSensor::getQuat() { + uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8); + + int16_t x, y, z, w; + x = y = z = w = 0; + + //Bit shift data into the right places and store it + w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); + x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); + y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); + z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); + + /*! + * Assign to Quaternion + * See + * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf + * 3.6.5.5 Orientation (Quaternion) + */ + const double scale = (1.0 / (1 << 14)); + imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); + return quat; +} + +int8_t IMUSensor::getTemp() { + int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR)); + return temp; +} + +void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) { + //Put into proper config for mapping stuff + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); + + //Return mode to operating mode + setModeTemp(currentMode); +} + +void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) { + //See above method, pretty much the exact same + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); + setModeTemp(currentMode); +} + +//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it. +void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) { + //Make sure IMU is on proper register page to get system status + writeRegister(BNO055_PAGE_ID_ADDR, 0); + + //If system status requested, read the status. + if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR); + //If self test result requested, pull the self test results. + if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR); + //Finally, if there's an error pull and stash it. + if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR); +} + +//Same as above method, byte pointers are fed into it as parameters that get populated by method. +void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) { + uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR); + if (sys != NULL) { + *sys = (calData >> 6) & 0x03; + } + if (gyro != NULL) { + *gyro = (calData >> 4) & 0x03; + } + if (accel != NULL) { + *accel = (calData >> 2) & 0x03; + } + if (mag != NULL) { + *mag = calData & 0x03; + } +} + +/* Functions to deal with raw calibration data */ +bool IMUSensor::getSensorOffsets(uint8_t *calibData) { + if (isFullyCalibrated()) { + setModeTemp(OPERATION_MODE_CONFIG); + + calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS); + + setModeTemp(currentMode); + return true; + } + return false; +} + +//Fully populated offset getter using type of offset, not just calibration data +bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) { + if (isFullyCalibrated()) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Accel offset range depends on the G-range: + +/-2g = +/- 2000 mg + +/-4g = +/- 4000 mg + +/-8g = +/- 8000 mg + +/-1§g = +/- 16000 mg */ + offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR)); + offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR)); + offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | + (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR)); + + /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ + offsets_type.mag_offset_x = + (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR)); + offsets_type.mag_offset_y = + (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR)); + offsets_type.mag_offset_z = + (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR)); + + /* Gyro offset range depends on the DPS range: + 2000 dps = +/- 32000 LSB + 1000 dps = +/- 16000 LSB + 500 dps = +/- 8000 LSB + 250 dps = +/- 4000 LSB + 125 dps = +/- 2000 LSB + ... where 1 DPS = 16 LSB */ + offsets_type.gyro_offset_x = + (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR)); + offsets_type.gyro_offset_y = + (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR)); + offsets_type.gyro_offset_z = + (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR)); + + /* Accelerometer radius = +/- 1000 LSB */ + offsets_type.accel_radius = + (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR)); + + /* Magnetometer radius = +/- 960 LSB */ + offsets_type.mag_radius = + (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR)); + + setModeTemp(currentMode); + return true; + } + return false; +} + +void IMUSensor::setSensorOffsets(const uint8_t *calibData) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + /* A writeLen() would make this much cleaner */ + writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); + writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); + writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); + writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); + writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); + writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); + + writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]); + writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]); + writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]); + writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]); + writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]); + writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]); + + writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]); + writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]); + writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]); + writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]); + writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]); + writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]); + + writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]); + writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]); + + writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]); + writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]); + + setModeTemp(currentMode); +} + +void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) { + setModeTemp(OPERATION_MODE_CONFIG); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); + writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); + writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); + writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); + writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); + writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); + + writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); + writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); + writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); + writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); + writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); + writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); + + writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); + writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); + writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); + writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); + writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); + writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); + + writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); + writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); + + writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); + writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); + + setModeTemp(currentMode); + +} + +bool IMUSensor::isFullyCalibrated() { + uint8_t system, gyro, accel, mag; + getCalibration(&system, &gyro, &accel, &mag); + + switch (currentMode) { + case OPERATION_MODE_ACCONLY: + return (accel == 3); + case OPERATION_MODE_MAGONLY: + return (mag == 3); + case OPERATION_MODE_GYRONLY: + case OPERATION_MODE_M4G: /* No magnetometer calibration required. */ + return (gyro == 3); + case OPERATION_MODE_ACCMAG: + case OPERATION_MODE_COMPASS: + return (accel == 3 && mag == 3); + case OPERATION_MODE_ACCGYRO: + case OPERATION_MODE_IMUPLUS: + return (accel == 3 && gyro == 3); + case OPERATION_MODE_MAGGYRO: + return (mag == 3 && gyro == 3); + default: + return (system == 3 && gyro == 3 && accel == 3 && mag == 3); + } +} + +/* Power managments functions */ +void IMUSensor::enterSuspendMode() { + /* Switch to config mode (just in case since this is the default) */ + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_PWR_MODE_ADDR, 0x02); + /* Set the requested operating mode (see section 3.3) */ + setModeTemp(currentMode); +} + +void IMUSensor::enterNormalMode() { + /* Switch to config mode (just in case since this is the default) */ + setModeTemp(OPERATION_MODE_CONFIG); + writeRegister(BNO055_PWR_MODE_ADDR, 0x00); + /* Set the requested operating mode (see section 3.3) */ + setModeTemp(modeback); +} + + + + + + + + + + diff --git a/src/unused/surfaceFitModel.cpp b/src/unused/surfaceFitModel.cpp new file mode 100644 index 0000000..d48da49 --- /dev/null +++ b/src/unused/surfaceFitModel.cpp @@ -0,0 +1,40 @@ +#include "../include/surfaceFitModel.hpp" + +SurfaceFitModel::SurfaceFitModel() { + + p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1); + + p(0, 0) = -781536.384794701; + p(1, 0) = 8623.59011973048; + p(0, 1) = 643.65918253; + p(2, 0) = -34.3646691281487; + p(1, 1) = -5.46066535343611; + p(0, 2) = -0.177121900557321; + p(3, 0) = 0.0573287698655951; + p(2, 1) = 0.0150031142038895; + p(1, 2) = 0.00101871763126609; + p(0, 3) = 1.63862900553892e-05; + p(4, 0) = -3.21785828407871e-05; + p(3, 1) = -1.3161091180883e-05; + p(2, 2) = -1.42505256569339e-06; + p(1, 3) = -4.76209793830867e-08; +} + + +double SurfaceFitModel::getFit(double x, double y) { + + return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) + + p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) + + p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) + + p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) + + p(1, 3) * x * pow(y, 3); +} + + + + + + + + + diff --git a/test/kalmanFilterTest.cpp b/test/kalmanFilterTest.cpp index e3dc1e4..bb0acda 100644 --- a/test/kalmanFilterTest.cpp +++ b/test/kalmanFilterTest.cpp @@ -1,6 +1,6 @@ #include <iostream> #include <gtest/gtest.h> -#include "eigen3/Eigen/Dense" +#include <Eigen/Dense> #include "../include/kalmanfilter.hpp" using namespace Eigen; |