diff options
| author | Dawsyn Schraiber <[email protected]> | 2024-06-13 14:30:58 -0400 |
|---|---|---|
| committer | GitHub <[email protected]> | 2024-06-13 14:30:58 -0400 |
| commit | 58b4bc754bbb9f5197119cd0c124e49c05acff46 (patch) | |
| tree | 8a65e23756374626e2c9cb997af9d8ed6f892390 /include | |
| parent | 8fbd08fe29bbc2246a78b481b219c241f62ff420 (diff) | |
| download | active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.gz active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.bz2 active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.zip | |
Where to begin…. (#13)
+/- Reworked collection of altimeter related functions into altimeter class
+/- Reworked bno055 class to be imu class with minimal functionality
\- Removed external Kalman filter implementations in favor of own in house version
\- Removed any/unused files
\+ Added buffer logger for when sitting on pad for extended period of time in effort to prevent filling of flash chip
\+ Added heartbeat LED for alive status
Diffstat (limited to 'include')
30 files changed, 248 insertions, 2947 deletions
diff --git a/include/SimpleKalmanFilter.h b/include/SimpleKalmanFilter.h deleted file mode 100644 index 61b682e..0000000 --- a/include/SimpleKalmanFilter.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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/actuationPlan.hpp b/include/actuationPlan.hpp deleted file mode 100644 index 8d1ef26..0000000 --- a/include/actuationPlan.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#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/actuator.hpp b/include/actuator.hpp deleted file mode 100644 index e7c4120..0000000 --- a/include/actuator.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#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/ads.hpp b/include/ads.hpp deleted file mode 100644 index b16c4fb..0000000 --- a/include/ads.hpp +++ /dev/null @@ -1,66 +0,0 @@ -#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/altimeter.hpp b/include/altimeter.hpp new file mode 100644 index 0000000..bf7effd --- /dev/null +++ b/include/altimeter.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "pico/stdlib.h" +#include "hardware/i2c.h" +#include "hardware/gpio.h" +#include <cstdint> + +class altimeter { + private: + uint8_t altitude_buffer[4]; + uint8_t buffer[4]; + uint8_t addr; + i2c_inst_t* inst; + public: + altimeter(i2c_inst_t* inst, uint8_t addr); + + void initialize(); + + void initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback); + + void set_threshold_altitude(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback); + + void unset_threshold_altitude(uint8_t interrupt_pin); + + float get_altitude_converted(); + + void get_altitude_raw(uint8_t* buffer); + + uint32_t expose_buffer(uint8_t** buffer); +}; + diff --git a/include/bno055.hpp b/include/bno055.hpp deleted file mode 100644 index 7fbac4d..0000000 --- a/include/bno055.hpp +++ /dev/null @@ -1,292 +0,0 @@ -#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/imu.hpp b/include/imu.hpp new file mode 100644 index 0000000..a1b72ef --- /dev/null +++ b/include/imu.hpp @@ -0,0 +1,207 @@ +#pragma once + +#include <stdint.h> +#include "hardware/i2c.h" + +#include <Eigen/Dense> + + +#define BNO055_NUM_OFFSET_REGISTERS 22 + + +typedef enum { + CONFIG = 0x00, + ACCELERATON_ONLY = 0x01, + MAGNETOMETER_ONLY = 0x02, + GYROSCOPE_ONLY = 0x03, + ACCEL_MAG = 0x04, + ACCEL_GYRO = 0x05, + MAG_GYRO = 0x06, + ACCEL_MAG_GYRO = 0x07, + IMU_PLUS = 0x08, + COMPASS = 0x09, + M4G = 0x0A, + NDOF_FMC_OFF = 0x0B, + NDOF = 0x0C +} imu_opmode_t; + +typedef enum { + PAGE_ID = 0x07, + + CHIP_ID = 0x00, + ACCEL_REV_ID = 0x01, + MAG_REV_ID = 0x02, + GYRO_REV_ID = 0x03, + SW_REV_ID_LSB = 0x04, + SW_REV_ID_MSB = 0x05, + BL_REV_ID = 0x06, + + ACCELERATION_X_LSB = 0x08, + ACCELERATION_X_MSB = 0x09, + ACCELERATION_Y_LSB = 0x0A, + ACCELERATION_Y_MSB = 0x0B, + ACCELERATION_Z_LSB = 0x0C, + ACCELERATION_Z_MSB = 0x0D, + + MAGNETOMETER_X_LSB = 0x0E, + MAGNETOMETER_X_MSB = 0x0F, + MAGNETOMETER_Y_LSB = 0x10, + MAGNETOMETER_Y_MSB = 0x11, + MAGNETOMETER_Z_LSB = 0x12, + MAGNETOMETER_Z_MSB = 0x13, + + GYROSCOPE_X_LSB = 0x14, + GYROSCOPE_X_MSB = 0x15, + GYROSCOPE_Y_LSB = 0x16, + GYROSCOPE_Y_MSB = 0x17, + GYROSCOPE_Z_LSB = 0x18, + GYROSCOPE_Z_MSB = 0x19, + + EULER_H_LSB = 0x1A, + EULER_H_MSB = 0x1B, + EULER_R_LSB = 0x1C, + EULER_R_MSB = 0x1D, + EULER_P_LSB = 0x1E, + EULER_P_MSB = 0x1F, + + QUATERNION_W_LSB = 0x20, + QUATERNION_W_MSB = 0x21, + QUATERNION_X_LSB = 0x22, + QUATERNION_X_MSB = 0x23, + QUATERNION_Y_LSB = 0x24, + QUATERNION_Y_MSB = 0x25, + QUATERNION_Z_LSB = 0x26, + QUATERNION_Z_MSB = 0x27, + + LINEAR_ACCELERATION_X_LSB = 0x28, + LINEAR_ACCELERATION_X_MSB = 0x29, + LINEAR_ACCELERATION_Y_LSB = 0x2A, + LINEAR_ACCELERATION_Y_MSB = 0x2B, + LINEAR_ACCELERATION_Z_LSB = 0x2C, + LINEAR_ACCELERATION_Z_MSB = 0x2D, + + GRAVITY_X_LSB = 0x2E, + GRAVITY_X_MSB = 0x2F, + GRAVITY_Y_LSB = 0x30, + GRAVITY_Y_MSB = 0x31, + GRAVITY_Z_LSB = 0x32, + GRAVITY_Z_MSB = 0x33, + + TEMPERATURE = 0x34, + + CALIBRATION_STATUS = 0x35, + SELF_TEST_RESULT = 0x36, + INTERRUPT_STATUS = 0x37, + + SYS_CLK_STATUS = 0x38, + SYS_STATUS = 0x39, + SYS_ERROR = 0x3A, + + UNIT_SELECTION = 0x3B, + + OPERATION_MODE = 0x3D, + POWER_MODE = 0x3E, + + SYS_TRIGGER = 0x3F, + TEMP_SOURCE = 0x40, + + AXIS_MAP_CONFIG = 0x41, + AXIS_MAP_SIGN = 0x42, + + SIC_MATRIX_0_LSB = 0x43, + SIC_MATRIX_0_MSB = 0x44, + SIC_MATRIX_1_LSB = 0x45, + SIC_MATRIX_1_MSB = 0x46, + SIC_MATRIX_2_LSB = 0x47, + SIC_MATRIX_2_MSB = 0x48, + SIC_MATRIX_3_LSB = 0x49, + SIC_MATRIX_3_MSB = 0x4A, + SIC_MATRIX_4_LSB = 0x4B, + SIC_MATRIX_4_MSB = 0x4C, + SIC_MATRIX_5_LSB = 0x4D, + SIC_MATRIX_5_MSB = 0x4E, + SIC_MATRIX_6_LSB = 0x4F, + SIC_MATRIX_6_MSB = 0x50, + SIC_MATRIX_7_LSB = 0x51, + SIC_MATRIX_7_MSB = 0x52, + SIC_MATRIX_8_LSB = 0x53, + SIC_MATRIX_8_MSB = 0x54, + + ACCELERATION_OFFSET_X_LSB = 0x55, + ACCELERATION_OFFSET_X_MSB = 0x56, + ACCELERATION_OFFSET_Y_LSB = 0x57, + ACCELERATION_OFFSET_Y_MSB = 0x58, + ACCELERATION_OFFSET_Z_LSB = 0x59, + ACCELERATION_OFFSET_Z_MSB = 0x5A, + + MAGNETOMETER_OFFSET_X_LSB = 0x5B, + MAGNETOMETER_OFFSET_X_MSB = 0x5C, + MAGNETOMETER_OFFSET_Y_LSB = 0x5D, + MAGNETOMETER_OFFSET_Y_MSB = 0x5E, + MAGNETOMETER_OFFSET_Z_LSB = 0x5F, + MAGNETOMETER_OFFSET_Z_MSB = 0x60, + + GYROSCOPE_OFFSET_X_LSB = 0x61, + GYROSCOPE_OFFSET_X_MSB = 0x62, + GYROSCOPE_OFFSET_Y_LSB = 0x63, + GYROSCOPE_OFFSET_Y_MSB = 0x64, + GYROSCOPE_OFFSET_Z_LSB = 0x65, + GYROSCOPE_OFFSET_Z_MSB = 0x66, + + ACCEL_RADIUS_LSB = 0x67, + ACCEL_RADIUS_MSB = 0x68, + MAG_RADIUS_LSB = 0x69, + MAG_RADIUS_MSB = 0x6A, + + RESET_INTERRUPT = 0x01, + + NO_MOTION_INTERRUPT = 0x00, + SLOW_NO_MOTION_INTERRUPT = 0x01, + THRESHOLD_INTERRUPT = 0x02, +} imu_reg_t; + +typedef enum { + NORMAL = 0x00, + LOW_POWER = 0x01, + SUSPEND = 0x02 +} imu_power_mode_t; + +typedef struct { + uint8_t sys; + uint8_t gyro; + uint8_t accel; + uint8_t mag; +} calibration_status_t ; + +class imu { + private: + i2c_inst_t* inst; + uint8_t addr; + uint8_t id; + imu_opmode_t mode; + + uint8_t buffer[10]; + uint8_t accel_buffer[6]; + uint8_t quat_buffer[8]; + + void read_register(uint8_t reg, size_t len, uint8_t* buffer); + + public: + imu(i2c_inst_t* inst, uint8_t addr, uint8_t id, imu_opmode_t mode); + + void initialize(); + + void reset(); + + void linear_acceleration(Eigen::Vector3f& vec); + + void quaternion(Eigen::Vector4f& vec); + + void quaternion_euler(Eigen::Vector3f& angles, Eigen::Vector4f& quat); + + void calibration_status(calibration_status_t* status); + + uint32_t expose_acceleration_buffer(uint8_t** buffer); + + uint32_t expose_quaternion_buffer(uint8_t** buffer); +}; diff --git a/include/kalmanfilter.hpp b/include/kalman_filter.hpp index 8b9e942..26d46cc 100644 --- a/include/kalmanfilter.hpp +++ b/include/kalman_filter.hpp @@ -1,11 +1,9 @@ #pragma once #include <Eigen/Dense> -#include <iostream> -#include <cmath> using namespace Eigen; -class KalmanFilter { +class kalman_filter { private: @@ -25,20 +23,20 @@ class KalmanFilter { int p; // Control Vector Dimension int m; // Measurement Vector Dimension - double dt; // timestep + float dt; // timestep /** * @brief Initialize all necessary matrices. * */ - void matrixInit(); + void matrix_initialize(); /** * @brief Update any existing variable elements in your State Transition * & Control Input matrices. * */ - void updateMatrices(); + void matrix_update(); /** * @brief Predict current State Vector & State Covariance @@ -47,7 +45,7 @@ class KalmanFilter { * @param control_vec The control input to be applied to the * previous State Vector */ - void prediction(VectorXf control_vec); + void predict(VectorXf control_vec); /** * @brief Correct the State Vector & State Covariance predictions @@ -59,7 +57,7 @@ class KalmanFilter { public: - KalmanFilter(); + kalman_filter(); /** * @brief Construct a new Kalman Filter object @@ -70,7 +68,7 @@ class KalmanFilter { * @param measurement_dim Measurement Vector Dimension. i.e. dim(z) * @param dt timestep */ - KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt); + kalman_filter(int state_dim, int control_dim, int measurement_dim, float dt); /** @@ -82,7 +80,7 @@ class KalmanFilter { * * @return Whether state initialization was successful */ - bool setInitialState(VectorXf state_vec, MatrixXf state_cov); + bool state_initialize(VectorXf state_vec, MatrixXf state_cov); /** * @brief Perform Kalman Filter operation with given control input vector @@ -94,5 +92,5 @@ class KalmanFilter { * * @return Filtered state vector */ - VectorXf run(VectorXf control, VectorXf measurement, double _dt); -};
\ No newline at end of file + VectorXf run(VectorXf control, VectorXf measurement, float _dt); +}; diff --git a/include/logger.hpp b/include/logger.hpp deleted file mode 100644 index 90fa05a..0000000 --- a/include/logger.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#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/math/imumath.h b/include/math/imumath.h deleted file mode 100644 index 831df60..0000000 --- a/include/math/imumath.h +++ /dev/null @@ -1,30 +0,0 @@ -// Inertial Measurement Unit Maths Library -// -// Copyright 2013-2021 Sam Cowen <[email protected]> -// -// Permission is hereby granted, free of charge, to any person obtaining a -// copy of this software and associated documentation files (the "Software"), -// to deal in the Software without restriction, including without limitation -// the rights to use, copy, modify, merge, publish, distribute, sublicense, -// and/or sell copies of the Software, and to permit persons to whom the -// Software is furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. - -#ifndef IMUMATH_H -#define IMUMATH_H - -#include "matrix.h" -#include "quaternion.h" -#include "vector.h" - -#endif
\ No newline at end of file diff --git a/include/math/matrix.h b/include/math/matrix.h deleted file mode 100644 index e71b2db..0000000 --- a/include/math/matrix.h +++ /dev/null @@ -1,185 +0,0 @@ -// Inertial Measurement Unit Maths Library -// -// Copyright 2013-2021 Sam Cowen <[email protected]> -// Bug fixes and cleanups by Gé Vissers ([email protected]) -// -// Permission is hereby granted, free of charge, to any person obtaining a -// copy of this software and associated documentation files (the "Software"), -// to deal in the Software without restriction, including without limitation -// the rights to use, copy, modify, merge, publish, distribute, sublicense, -// and/or sell copies of the Software, and to permit persons to whom the -// Software is furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. - -#ifndef IMUMATH_MATRIX_HPP -#define IMUMATH_MATRIX_HPP - -#include <stdint.h> -#include <string.h> - -#include "vector.h" - -namespace imu { - -template <uint8_t N> class Matrix { -public: - Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); } - - Matrix(const Matrix &m) { - for (int ij = 0; ij < N * N; ++ij) { - _cell_data[ij] = m._cell_data[ij]; - } - } - - ~Matrix() {} - - Matrix &operator=(const Matrix &m) { - for (int ij = 0; ij < N * N; ++ij) { - _cell_data[ij] = m._cell_data[ij]; - } - return *this; - } - - Vector<N> row_to_vector(int i) const { - Vector<N> ret; - for (int j = 0; j < N; j++) { - ret[j] = cell(i, j); - } - return ret; - } - - Vector<N> col_to_vector(int j) const { - Vector<N> ret; - for (int i = 0; i < N; i++) { - ret[i] = cell(i, j); - } - return ret; - } - - void vector_to_row(const Vector<N> &v, int i) { - for (int j = 0; j < N; j++) { - cell(i, j) = v[j]; - } - } - - void vector_to_col(const Vector<N> &v, int j) { - for (int i = 0; i < N; i++) { - cell(i, j) = v[i]; - } - } - - double operator()(int i, int j) const { return cell(i, j); } - double &operator()(int i, int j) { return cell(i, j); } - - double cell(int i, int j) const { return _cell_data[i * N + j]; } - double &cell(int i, int j) { return _cell_data[i * N + j]; } - - Matrix operator+(const Matrix &m) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij]; - } - return ret; - } - - Matrix operator-(const Matrix &m) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij]; - } - return ret; - } - - Matrix operator*(double scalar) const { - Matrix ret; - for (int ij = 0; ij < N * N; ++ij) { - ret._cell_data[ij] = _cell_data[ij] * scalar; - } - return ret; - } - - Matrix operator*(const Matrix &m) const { - Matrix ret; - for (int i = 0; i < N; i++) { - Vector<N> row = row_to_vector(i); - for (int j = 0; j < N; j++) { - ret(i, j) = row.dot(m.col_to_vector(j)); - } - } - return ret; - } - - Matrix transpose() const { - Matrix ret; - for (int i = 0; i < N; i++) { - for (int j = 0; j < N; j++) { - ret(j, i) = cell(i, j); - } - } - return ret; - } - - Matrix<N - 1> minor_matrix(int row, int col) const { - Matrix<N - 1> ret; - for (int i = 0, im = 0; i < N; i++) { - if (i == row) - continue; - - for (int j = 0, jm = 0; j < N; j++) { - if (j != col) { - ret(im, jm++) = cell(i, j); - } - } - im++; - } - return ret; - } - - double determinant() const { - // specialization for N == 1 given below this class - double det = 0.0, sign = 1.0; - for (int i = 0; i < N; ++i, sign = -sign) - det += sign * cell(0, i) * minor_matrix(0, i).determinant(); - return det; - } - - Matrix invert() const { - Matrix ret; - double det = determinant(); - - for (int i = 0; i < N; i++) { - for (int j = 0; j < N; j++) { - ret(i, j) = minor_matrix(j, i).determinant() / det; - if ((i + j) % 2 == 1) - ret(i, j) = -ret(i, j); - } - } - return ret; - } - - double trace() const { - double tr = 0.0; - for (int i = 0; i < N; ++i) - tr += cell(i, i); - return tr; - } - -private: - double _cell_data[N * N]; -}; - -template <> inline double Matrix<1>::determinant() const { return cell(0, 0); } - -}; // namespace imu - -#endif
\ No newline at end of file diff --git a/include/math/quaternion.h b/include/math/quaternion.h deleted file mode 100644 index c5b907a..0000000 --- a/include/math/quaternion.h +++ /dev/null @@ -1,214 +0,0 @@ -// Inertial Measurement Unit Maths Library -// -// Copyright 2013-2021 Sam Cowen <[email protected]> -// Bug fixes and cleanups by Gé Vissers ([email protected]) -// -// Permission is hereby granted, free of charge, to any person obtaining a -// copy of this software and associated documentation files (the "Software"), -// to deal in the Software without restriction, including without limitation -// the rights to use, copy, modify, merge, publish, distribute, sublicense, -// and/or sell copies of the Software, and to permit persons to whom the -// Software is furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. - -#ifndef IMUMATH_QUATERNION_HPP -#define IMUMATH_QUATERNION_HPP - -#include <math.h> -#include <stdint.h> -#include <stdlib.h> -#include <string.h> - -#include "matrix.h" - -namespace imu { - -class Quaternion { -public: - Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} - - Quaternion(double w, double x, double y, double z) - : _w(w), _x(x), _y(y), _z(z) {} - - Quaternion(double w, Vector<3> vec) - : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} - - double &w() { return _w; } - double &x() { return _x; } - double &y() { return _y; } - double &z() { return _z; } - - double w() const { return _w; } - double x() const { return _x; } - double y() const { return _y; } - double z() const { return _z; } - - double magnitude() const { - return sqrt(_w * _w + _x * _x + _y * _y + _z * _z); - } - - void normalize() { - double mag = magnitude(); - *this = this->scale(1 / mag); - } - - Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); } - - void fromAxisAngle(const Vector<3> &axis, double theta) { - _w = cos(theta / 2); - // only need to calculate sine of half theta once - double sht = sin(theta / 2); - _x = axis.x() * sht; - _y = axis.y() * sht; - _z = axis.z() * sht; - } - - void fromMatrix(const Matrix<3> &m) { - double tr = m.trace(); - - double S; - if (tr > 0) { - S = sqrt(tr + 1.0) * 2; - _w = 0.25 * S; - _x = (m(2, 1) - m(1, 2)) / S; - _y = (m(0, 2) - m(2, 0)) / S; - _z = (m(1, 0) - m(0, 1)) / S; - } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { - S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; - _w = (m(2, 1) - m(1, 2)) / S; - _x = 0.25 * S; - _y = (m(0, 1) + m(1, 0)) / S; - _z = (m(0, 2) + m(2, 0)) / S; - } else if (m(1, 1) > m(2, 2)) { - S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; - _w = (m(0, 2) - m(2, 0)) / S; - _x = (m(0, 1) + m(1, 0)) / S; - _y = 0.25 * S; - _z = (m(1, 2) + m(2, 1)) / S; - } else { - S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; - _w = (m(1, 0) - m(0, 1)) / S; - _x = (m(0, 2) + m(2, 0)) / S; - _y = (m(1, 2) + m(2, 1)) / S; - _z = 0.25 * S; - } - } - - void toAxisAngle(Vector<3> &axis, double &angle) const { - double sqw = sqrt(1 - _w * _w); - if (sqw == 0) // it's a singularity and divide by zero, avoid - return; - - angle = 2 * acos(_w); - axis.x() = _x / sqw; - axis.y() = _y / sqw; - axis.z() = _z / sqw; - } - - Matrix<3> toMatrix() const { - Matrix<3> ret; - ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z; - ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z; - ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y; - - ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z; - ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z; - ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x; - - ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y; - ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x; - ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y; - return ret; - } - - // Returns euler angles that represent the quaternion. Angles are - // returned in rotation order and right-handed about the specified - // axes: - // - // v[0] is applied 1st about z (ie, roll) - // v[1] is applied 2nd about y (ie, pitch) - // v[2] is applied 3rd about x (ie, yaw) - // - // Note that this means result.x() is not a rotation about x; - // similarly for result.z(). - // - Vector<3> toEuler() const { - Vector<3> ret; - double sqw = _w * _w; - double sqx = _x * _x; - double sqy = _y * _y; - double sqz = _z * _z; - - ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)); - ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)); - ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)); - - return ret; - } - - Vector<3> toAngularVelocity(double dt) const { - Vector<3> ret; - Quaternion one(1.0, 0.0, 0.0, 0.0); - Quaternion delta = one - *this; - Quaternion r = (delta / dt); - r = r * 2; - r = r * one; - - ret.x() = r.x(); - ret.y() = r.y(); - ret.z() = r.z(); - return ret; - } - - Vector<3> rotateVector(const Vector<2> &v) const { - return rotateVector(Vector<3>(v.x(), v.y())); - } - - Vector<3> rotateVector(const Vector<3> &v) const { - Vector<3> qv(_x, _y, _z); - Vector<3> t = qv.cross(v) * 2.0; - return v + t * _w + qv.cross(t); - } - - Quaternion operator*(const Quaternion &q) const { - return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z, - _w * q._x + _x * q._w + _y * q._z - _z * q._y, - _w * q._y - _x * q._z + _y * q._w + _z * q._x, - _w * q._z + _x * q._y - _y * q._x + _z * q._w); - } - - Quaternion operator+(const Quaternion &q) const { - return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); - } - - Quaternion operator-(const Quaternion &q) const { - return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); - } - - Quaternion operator/(double scalar) const { - return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); - } - - Quaternion operator*(double scalar) const { return scale(scalar); } - - Quaternion scale(double scalar) const { - return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); - } - -private: - double _w, _x, _y, _z; -}; - -} // namespace imu - -#endif
\ No newline at end of file diff --git a/include/math/vector.h b/include/math/vector.h deleted file mode 100644 index 10345c3..0000000 --- a/include/math/vector.h +++ /dev/null @@ -1,184 +0,0 @@ -// Inertial Measurement Unit Maths Library -// -// Copyright 2013-2021 Sam Cowen <[email protected]> -// Bug fixes and cleanups by Gé Vissers ([email protected]) -// -// Permission is hereby granted, free of charge, to any person obtaining a -// copy of this software and associated documentation files (the "Software"), -// to deal in the Software without restriction, including without limitation -// the rights to use, copy, modify, merge, publish, distribute, sublicense, -// and/or sell copies of the Software, and to permit persons to whom the -// Software is furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -// DEALINGS IN THE SOFTWARE. - -#ifndef IMUMATH_VECTOR_HPP -#define IMUMATH_VECTOR_HPP - -#include <math.h> -#include <stdint.h> -#include <string.h> - -namespace imu { - -template <uint8_t N> class Vector { -public: - Vector() { memset(p_vec, 0, sizeof(double) * N); } - - Vector(double a) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - } - - Vector(double a, double b) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - } - - Vector(double a, double b, double c) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - p_vec[2] = c; - } - - Vector(double a, double b, double c, double d) { - memset(p_vec, 0, sizeof(double) * N); - p_vec[0] = a; - p_vec[1] = b; - p_vec[2] = c; - p_vec[3] = d; - } - - Vector(const Vector<N> &v) { - for (int x = 0; x < N; x++) - p_vec[x] = v.p_vec[x]; - } - - ~Vector() {} - - uint8_t n() { return N; } - - double magnitude() const { - double res = 0; - for (int i = 0; i < N; i++) - res += p_vec[i] * p_vec[i]; - - return sqrt(res); - } - - void normalize() { - double mag = magnitude(); - if (isnan(mag) || mag == 0.0) - return; - - for (int i = 0; i < N; i++) - p_vec[i] /= mag; - } - - double dot(const Vector &v) const { - double ret = 0; - for (int i = 0; i < N; i++) - ret += p_vec[i] * v.p_vec[i]; - - return ret; - } - - // The cross product is only valid for vectors with 3 dimensions, - // with the exception of higher dimensional stuff that is beyond - // the intended scope of this library. - // Only a definition for N==3 is given below this class, using - // cross() with another value for N will result in a link error. - Vector cross(const Vector &v) const; - - Vector scale(double scalar) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] * scalar; - return ret; - } - - Vector invert() const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = -p_vec[i]; - return ret; - } - - Vector &operator=(const Vector &v) { - for (int x = 0; x < N; x++) - p_vec[x] = v.p_vec[x]; - return *this; - } - - double &operator[](int n) { return p_vec[n]; } - - double operator[](int n) const { return p_vec[n]; } - - double &operator()(int n) { return p_vec[n]; } - - double operator()(int n) const { return p_vec[n]; } - - Vector operator+(const Vector &v) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] + v.p_vec[i]; - return ret; - } - - Vector operator-(const Vector &v) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] - v.p_vec[i]; - return ret; - } - - Vector operator*(double scalar) const { return scale(scalar); } - - Vector operator/(double scalar) const { - Vector ret; - for (int i = 0; i < N; i++) - ret.p_vec[i] = p_vec[i] / scalar; - return ret; - } - - void toDegrees() { - for (int i = 0; i < N; i++) - p_vec[i] *= 57.2957795131; // 180/pi - } - - void toRadians() { - for (int i = 0; i < N; i++) - p_vec[i] *= 0.01745329251; // pi/180 - } - - double &x() { return p_vec[0]; } - double &y() { return p_vec[1]; } - double &z() { return p_vec[2]; } - double x() const { return p_vec[0]; } - double y() const { return p_vec[1]; } - double z() const { return p_vec[2]; } - -private: - double p_vec[N]; -}; - -template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { - return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], - p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], - p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); -} - -} // namespace imu - -#endif
\ No newline at end of file diff --git a/include/motor.hpp b/include/motor.hpp deleted file mode 100644 index cb95b6a..0000000 --- a/include/motor.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#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/rocketUtils.hpp b/include/rocketUtils.hpp deleted file mode 100644 index 65b9cea..0000000 --- a/include/rocketUtils.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#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/sensorAltimeter.hpp b/include/sensorAltimeter.hpp deleted file mode 100644 index aa3a3cd..0000000 --- a/include/sensorAltimeter.hpp +++ /dev/null @@ -1,165 +0,0 @@ -#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/sensorI2C.hpp b/include/sensorI2C.hpp deleted file mode 100644 index bd497a5..0000000 --- a/include/sensorI2C.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#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/sensorIMU.hpp b/include/sensorIMU.hpp deleted file mode 100644 index caddbe4..0000000 --- a/include/sensorIMU.hpp +++ /dev/null @@ -1,312 +0,0 @@ -#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/state_machine.hpp b/include/state_machine.hpp deleted file mode 100644 index 874fc08..0000000 --- a/include/state_machine.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#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/surfaceFitModel.hpp b/include/surfaceFitModel.hpp deleted file mode 100644 index 303633f..0000000 --- a/include/surfaceFitModel.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#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/include/unused/actuationPlan.hpp b/include/unused/actuationPlan.hpp deleted file mode 100644 index 8d1ef26..0000000 --- a/include/unused/actuationPlan.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#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 deleted file mode 100644 index e7c4120..0000000 --- a/include/unused/actuator.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#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 deleted file mode 100644 index b16c4fb..0000000 --- a/include/unused/ads.hpp +++ /dev/null @@ -1,66 +0,0 @@ -#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 deleted file mode 100644 index 90fa05a..0000000 --- a/include/unused/logger.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#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 deleted file mode 100644 index cb95b6a..0000000 --- a/include/unused/motor.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#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 deleted file mode 100644 index 65b9cea..0000000 --- a/include/unused/rocketUtils.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#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 deleted file mode 100644 index aa3a3cd..0000000 --- a/include/unused/sensorAltimeter.hpp +++ /dev/null @@ -1,165 +0,0 @@ -#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 deleted file mode 100644 index bd497a5..0000000 --- a/include/unused/sensorI2C.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#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 deleted file mode 100644 index caddbe4..0000000 --- a/include/unused/sensorIMU.hpp +++ /dev/null @@ -1,312 +0,0 @@ -#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 deleted file mode 100644 index 303633f..0000000 --- a/include/unused/surfaceFitModel.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#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); -}; |