diff --git a/MIDAS/platformio.ini b/MIDAS/platformio.ini index 2f52aeb4..851f0862 100644 --- a/MIDAS/platformio.ini +++ b/MIDAS/platformio.ini @@ -82,6 +82,8 @@ build_flags = build_src_filter = +<*> + - - build_unflags = -std=gnu++11 +lib_ignore = + TCAL9539 [env:mcu_silsim_booster] platform = native @@ -96,7 +98,7 @@ build_flags = build_src_filter = +<*> + - - build_unflags = -std=gnu++11 -lib_deps = - Eigen +;lib_deps = +; Eigen lib_ignore = TCAL9539 diff --git a/MIDAS/src/Mutex.h b/MIDAS/src/Mutex.h index a524ddaa..3e18eb6b 100644 --- a/MIDAS/src/Mutex.h +++ b/MIDAS/src/Mutex.h @@ -1,10 +1,4 @@ -#ifdef SILSIM -#include "silsim/emulation.h" -#else -#include -#include -#include -#endif +#include "hal.h" #define MUTEX_TIMEOUT pdMS_TO_TICKS(100) @@ -50,10 +44,10 @@ struct Mutex { * @return The value in the mutex. */ T read() { - if (!mutex_handle || mutex_handle != check) { - Serial.println("Aw shucks"); - Serial.flush(); - } +// if (!mutex_handle || mutex_handle != check) { +// Serial.println("Aw shucks"); +// Serial.flush(); +// } xSemaphoreTake(mutex_handle, portMAX_DELAY); // while (!xSemaphoreTake(mutex_handle, MUTEX_TIMEOUT)) { } T ret = data; @@ -76,10 +70,10 @@ struct Mutex { * @param ptr buffer to store data in */ void read2(T* ptr) { - if (!mutex_handle || mutex_handle != check) { - Serial.println("Aw shucks"); - Serial.flush(); - } +// if (!mutex_handle || mutex_handle != check) { +// Serial.println("Aw shucks"); +// Serial.flush(); +// } xSemaphoreTake(mutex_handle, portMAX_DELAY); // while (!xSemaphoreTake(mutex_handle, MUTEX_TIMEOUT)) { } *ptr = data; @@ -92,10 +86,10 @@ struct Mutex { * @param value What to update the mutex to. */ void write(T value) { - if (!mutex_handle || mutex_handle != check) { - Serial.println("Aw shucks"); - Serial.flush(); - } +// if (!mutex_handle || mutex_handle != check) { +// Serial.println("Aw shucks"); +// Serial.flush(); +// } while (!xSemaphoreTake(mutex_handle, MUTEX_TIMEOUT)) { } data = value; diff --git a/MIDAS/src/Queue.h b/MIDAS/src/Queue.h index ffd8ce3c..e431997b 100644 --- a/MIDAS/src/Queue.h +++ b/MIDAS/src/Queue.h @@ -43,8 +43,8 @@ class Queue { */ void send(T value) { if(xQueueSendToBack(queue, &value, 0) == errQUEUE_FULL){ - Serial.print("FULL QUEUE"); - Serial.println(pcTaskGetName(nullptr)); +// Serial.print("FULL QUEUE"); +// Serial.println(pcTaskGetName(nullptr)); } } diff --git a/MIDAS/src/b2b_interface.cpp b/MIDAS/src/b2b_interface.cpp deleted file mode 100644 index 5de996f7..00000000 --- a/MIDAS/src/b2b_interface.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "b2b_interface.h" - - -ErrorCode B2BInterface::init() { - // No special init - return ErrorCode::NoError; -} - -uint8_t CameraB2B::read() { - #ifdef B2B_I2C - Wire.requestFrom(0x69, 1); - uint8_t res = Wire.read(); - return res; - #endif - - return 0xFF; -} - -/** - * @brief Transmits the given CameraCommand over I2C / CAN (depending on which interface type is defined) - */ -void CameraB2B::transmit_command(CameraCommand command) { - #ifdef B2B_I2C - - Wire.beginTransmission(0x69); // 0x69 --> Camera board i2c address - Wire.write((uint8_t) command); - if (Wire.endTransmission()) { - Serial.println("Camera B2B i2c write error"); - } - - #endif - - #ifdef B2B_CAN - // todo :D - #endif -} - -/** - * @brief Transmits command to toggle on the video transmitter - */ -void CameraB2B::vtx_on() { - transmit_command(CameraCommand::VTX_ON); - vtx_state_ = true; -} - -/** - * @brief Transmits command to toggle off the video transmitter - */ -void CameraB2B::vtx_off() { - transmit_command(CameraCommand::VTX_OFF); - vtx_state_ = false; -} - -/** - * @brief Transmits command to toggle the video transmitter - */ -void CameraB2B::vtx_toggle() { - if(vtx_state_) { - vtx_off(); - } else { - vtx_on(); - } -} - -/** - * @brief Transmits command to set which camera is currently active on the video multiplexer - */ -void CameraB2B::vmux_set(int cam_select) { - if(cam_select) { - // If cam_select is 1, switch to MUX 2 - transmit_command(CameraCommand::MUX_2); - mux_select_ = true; - } else { - // Otherwise switch to MUX 1 - transmit_command(CameraCommand::MUX_1); - mux_select_ = false; - } -} - -/** - * @brief Transmits command to toggle which camera is currently active on the video multiplexer - */ -void CameraB2B::vmux_toggle() { - vmux_set(!mux_select_); -} - -/** - * @brief Transmits command to enable power to a camera - */ -void CameraB2B::camera_on(int cam_index) { - switch (cam_index) { - case 0: - transmit_command(CameraCommand::CAMERA1_ON); - cam_state_[0] = true; - break; - case 1: - transmit_command(CameraCommand::CAMERA2_ON); - cam_state_[1] = true; - break; - default: - Serial.print("B2B camera on -- invalid index "); - Serial.println(cam_index); - break; - } -} - -/** - * @brief Transmits command to disable power to a camera - * - * If the camera was previously on, it will first stop recording, then power off. - */ -void CameraB2B::camera_off(int cam_index) { - switch (cam_index) { - case 0: - transmit_command(CameraCommand::CAMERA1_OFF); - cam_state_[0] = false; - break; - case 1: - transmit_command(CameraCommand::CAMERA2_OFF); - cam_state_[1] = false; - break; - default: - Serial.print("B2B camera off -- invalid index "); - Serial.println(cam_index); - break; - } -} - -/** - * @brief Transmits command to toggle power to a camera - */ -void CameraB2B::camera_toggle(int cam_index) { - switch (cam_index) { - case 0: - if (cam_state_[0]) { - camera_off(0); - } else { - camera_on(0); - } - break; - case 1: - if (cam_state_[1]) { - camera_off(1); - } else { - camera_on(1); - } - break; - default: - Serial.print("B2B camera toggle -- invalid index "); - Serial.println(cam_index); - break; - } -} \ No newline at end of file diff --git a/MIDAS/src/b2b_interface.h b/MIDAS/src/b2b_interface.h deleted file mode 100644 index 1fbecdee..00000000 --- a/MIDAS/src/b2b_interface.h +++ /dev/null @@ -1,64 +0,0 @@ -#include -#include -#include -#include "errors.h" -#include "hal.h" - -// Which b2b communication we should use -#define B2B_I2C -// #define B2B_CAN - -#if defined(B2B_I2C) && defined(B2B_CAN) -#error "B2B can only use one option of B2B_I2C, or B2B_CAN" -#elif !defined(B2B_I2C) && !defined(B2B_CAN) -#error "At least one B2B_I2C or B2B_CAN must be defined" -#endif - -#define CAM_1 0 -#define CAM_2 1 - -#define SIDE_CAMERA CAM_1 -#define BULKHEAD_CAMERA CAM_2 - - -enum class CameraCommand { - CAMERA1_OFF = 0, - CAMERA1_ON = 1, - CAMERA2_OFF = 2, - CAMERA2_ON = 3, - VTX_OFF = 4, - VTX_ON = 5, - MUX_1 = 6, - MUX_2 = 7 -}; - -struct CameraB2B { - - void camera_on(int cam_index); - void camera_off(int cam_index); - void camera_toggle(int cam_index); - - void vtx_on(); - void vtx_off(); - void vtx_toggle(); - - void vmux_set(int cam_select); - void vmux_toggle(); - - uint8_t read(); - - private: - void transmit_command(CameraCommand command); - bool cam_state_[2] = { false, false }; // false: off, true: on - bool vtx_state_ = false; - bool mux_select_ = false; // false: VMUX 1, true: VMUX 2 -}; - -/** - * @struct Interface for B2B communication protocol - */ -struct B2BInterface { - ErrorCode init(); - - CameraB2B camera; -}; diff --git a/MIDAS/src/data_logging.cpp b/MIDAS/src/data_logging.cpp index e6cb5ce2..4849e9f3 100644 --- a/MIDAS/src/data_logging.cpp +++ b/MIDAS/src/data_logging.cpp @@ -14,14 +14,14 @@ constexpr ReadingDiscriminant get_discriminant(); #define ASSOCIATE(ty, id) template<> constexpr ReadingDiscriminant get_discriminant() { return ReadingDiscriminant::id; } ASSOCIATE(LowGData, ID_LOWG) -ASSOCIATE(LowGLSM, ID_LOWGLSM) +ASSOCIATE(LowGLSMData, ID_LOWGLSM) ASSOCIATE(HighGData, ID_HIGHG) -ASSOCIATE(Barometer, ID_BAROMETER) -ASSOCIATE(Continuity, ID_CONTINUITY) -ASSOCIATE(Voltage, ID_VOLTAGE) -ASSOCIATE(GPS, ID_GPS) -ASSOCIATE(Magnetometer, ID_MAGNETOMETER) -ASSOCIATE(Orientation, ID_ORIENTATION) +ASSOCIATE(BarometerData, ID_BAROMETER) +ASSOCIATE(ContinuityData, ID_CONTINUITY) +ASSOCIATE(VoltageData, ID_VOLTAGE) +ASSOCIATE(GPSData, ID_GPS) +ASSOCIATE(MagnetometerData, ID_MAGNETOMETER) +ASSOCIATE(OrientationData, ID_ORIENTATION) ASSOCIATE(FSMState, ID_FSM) ASSOCIATE(KalmanData, ID_KALMAN) ASSOCIATE(PyroState, ID_PYRO) diff --git a/MIDAS/src/errors.cpp b/MIDAS/src/errors.cpp index e21eb59a..827a2863 100644 --- a/MIDAS/src/errors.cpp +++ b/MIDAS/src/errors.cpp @@ -1,5 +1,4 @@ #include "errors.h" -#include "TCAL9539.h" /** * If an error during initialization was detected, some combination of the blue, green, orange, and red LEDs will be on, @@ -118,6 +117,6 @@ void update_error_LED(ErrorCode error) { void update_error_LED(ErrorCode error) { -}; +} #endif diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index c434abed..a4163c43 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -62,13 +62,13 @@ StateEstimate::StateEstimate(RocketData& state) { acceleration = sensor_average(state.high_g, [](HighGData& data) { return (double) data.ax; }); - altitude = sensor_average(state.barometer, [](Barometer& data) { + altitude = sensor_average(state.barometer, [](BarometerData& data) { return (double) data.altitude; }); jerk = sensor_derivative(state.high_g, [](HighGData& data) { return (double) data.ax; }); - vertical_speed = sensor_derivative(state.barometer, [](Barometer& data) { + vertical_speed = sensor_derivative(state.barometer, [](BarometerData& data) { return (double) data.altitude; }); } diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 0af00aa8..6636e0f2 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,9 +1,9 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" +#include "hal.h" -EKF::EKF() : KalmanFilter() -{ -state = KalmanData(); +EKF::EKF() : KalmanFilter() { + state = KalmanData(); } // constants @@ -16,8 +16,7 @@ const float height_sustainer = 2.029; // (m) height of rocket Sustainer const float mass_full = 33.84; // (kg) Sustainer + Booster const float mass_sustainer = 10.93; // (kg) Sustainer -typedef struct -{ +typedef struct { float mach; float alpha; float CA_power_on; @@ -27,31 +26,31 @@ typedef struct // stores the aerodynamic coefficients for the corresponding Mach number const AeroCoeff aero_data[] = { - {0.04,0,1.000001789,25.80486518,123.856999}, - {0.08,0,0.899149955,25.80486518,123.856999}, - {0.12,0,0.848262793,25.80486518,123.856999}, - {0.16,0,0.815490497,25.80486518,123.856999}, - {0.2,0,0.791977907,25.80486518,123.856999}, - {0.24,0,0.77407759,25.80486518,123.856999}, - {0.28,0,0.763046286,25.80486518,123.856999}, - {0.32,0,0.758319846,25.80486518,123.856999}, - {0.36,0,0.760511343,25.80486518,123.856999}, - {0.4,0,0.763737136,25.80486518,123.856999}, - {0.44,0,0.767325178,25.80486518,123.856999}, - {0.48,0,0.771334851,25.80486518,123.856999}, - {0.52,0,0.775843406,25.80486518,123.856999}, - {0.56,0,0.780953377,25.80486518,123.856999}, - {0.6,0,0.785771581,25.80486518,123.856999}, - {0.64,0,0.793730593,25.80486518,123.856999}, - {0.68,0,0.80285965,25.80486518,123.856999}, - {0.72,0,0.807910063,25.80486518,123.856999}, - {0.76,0,0.807403195,25.80486518,123.856999}, - {0.8,0,0.806889479,25.80486518,123.856999}, - {0.84,0,0.832707826,25.80486518,123.856999}, - {0.88,0,0.858519521,25.80486518,123.856999}, - {0.92,0,0.895125486,25.492166,124.3408619}, - {0.96,0,0.923744595,24.86676763,125.3085876}, - {1,0,0.941214699,24.24136926,126.2763132}, + {0.04, 0, 1.000001789, 25.80486518, 123.856999}, + {0.08, 0, 0.899149955, 25.80486518, 123.856999}, + {0.12, 0, 0.848262793, 25.80486518, 123.856999}, + {0.16, 0, 0.815490497, 25.80486518, 123.856999}, + {0.2, 0, 0.791977907, 25.80486518, 123.856999}, + {0.24, 0, 0.77407759, 25.80486518, 123.856999}, + {0.28, 0, 0.763046286, 25.80486518, 123.856999}, + {0.32, 0, 0.758319846, 25.80486518, 123.856999}, + {0.36, 0, 0.760511343, 25.80486518, 123.856999}, + {0.4, 0, 0.763737136, 25.80486518, 123.856999}, + {0.44, 0, 0.767325178, 25.80486518, 123.856999}, + {0.48, 0, 0.771334851, 25.80486518, 123.856999}, + {0.52, 0, 0.775843406, 25.80486518, 123.856999}, + {0.56, 0, 0.780953377, 25.80486518, 123.856999}, + {0.6, 0, 0.785771581, 25.80486518, 123.856999}, + {0.64, 0, 0.793730593, 25.80486518, 123.856999}, + {0.68, 0, 0.80285965, 25.80486518, 123.856999}, + {0.72, 0, 0.807910063, 25.80486518, 123.856999}, + {0.76, 0, 0.807403195, 25.80486518, 123.856999}, + {0.8, 0, 0.806889479, 25.80486518, 123.856999}, + {0.84, 0, 0.832707826, 25.80486518, 123.856999}, + {0.88, 0, 0.858519521, 25.80486518, 123.856999}, + {0.92, 0, 0.895125486, 25.492166, 124.3408619}, + {0.96, 0, 0.923744595, 24.86676763, 125.3085876}, + {1, 0, 0.941214699, 24.24136926, 126.2763132}, }; // Number of entries @@ -145,34 +144,28 @@ std::map O5500X_data = { * be kept consistent (do not mix GPS altitude and barometric). * */ -void EKF::initialize(RocketSystems *args) -{ - Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); +void EKF::initialize(RocketData& data) { float sum = 0; - for (int i = 0; i < 30; i++) - { - Barometer barometer = args->rocket_data.barometer.getRecent(); - LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); - Acceleration accelerations = { - .ax = initial_accelerometer.ax, - .ay = initial_accelerometer.ay, - .az = initial_accelerometer.az}; - sum += barometer.altitude; - - init_accel(0, 0) += accelerations.az; - init_accel(1, 0) += accelerations.ay; - init_accel(2, 0) += -accelerations.ax; - THREAD_SLEEP(100); + for (int i = 0; i < 30; i++) { + BarometerData barometer = data.barometer.getRecent(); + LowGData initial_accelerometer = data.low_g.getRecent(); + Acceleration accelerations = { + .ax = initial_accelerometer.ax, + .ay = initial_accelerometer.ay, + .az = initial_accelerometer.az}; + sum += barometer.altitude; + + init_accel(0, 0) += accelerations.az; + init_accel(1, 0) += accelerations.ay; + init_accel(2, 0) += -accelerations.ax; + THREAD_SLEEP(100); } init_accel(0, 0) /= 30; init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; - euler_t euler = orientation.getEuler(); - euler.yaw = -euler.yaw; - // set x_k x_k(0, 0) = sum / 30; x_k(3, 0) = 0; @@ -236,25 +229,22 @@ void EKF::initialize(RocketSystems *args) * it extrapolates the state at time n+1 based on the state at time n. */ -void EKF::priori(float dt, Orientation &orientation, FSMState fsm) -{ +void EKF::priori(float dt, OrientationData& orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); - Velocity omega = orientation.getVelocity(); - euler_t angles = orientation.getEuler(); + Velocity omega = orientation.orientation_velocity; + euler_t angles = orientation.euler; // Eigen::Matrix gravity = Eigen::Matrix::Zero(); - if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED)) - { - gravity(0, 0) = -9.81; + if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED)) { + gravity(0, 0) = -9.81; } else { - gravity(0, 0) = 0; + gravity(0, 0) = 0; } float m = mass_sustainer; float h = height_sustainer; - if (fsm < FSMState::STATE_BURNOUT) - { - m = mass_full; - h = height_full; + if (fsm < FSMState::STATE_BURNOUT) { + m = mass_full; + h = height_full; } float w_x = omega.vx; @@ -270,11 +260,12 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) float velocity_magnitude = pow(vel_mag_squared, 0.5); float mach = velocity_magnitude / a; int index = std::round(mach / 0.04); - index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); + index = std::clamp(index, 0, (int) AERO_DATA_SIZE - 1); Ca = aero_data[index].CA_power_on; float Fax = -0.5 * rho * (vel_mag_squared) * float(Ca) * (pi * r * r); - float Fay = 0; float Faz = 0; + float Fay = 0; + float Faz = 0; Eigen::Matrix Fg_body; EKF::GlobalToBody(angles, gravity); @@ -295,16 +286,16 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) xdot << x_k(1, 0), - (Fax + Ftx + Fgx) / m - (w_y * x_k(7, 0) - w_z * x_k(4, 0)), - 1.0, + (Fax + Ftx + Fgx) / m - (w_y * x_k(7, 0) - w_z * x_k(4, 0)), + 1.0, - x_k(4, 0), - (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7, 0)), - 1.0, + x_k(4, 0), + (Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7, 0)), + 1.0, - x_k(7, 0), - (Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)), - 1.0; + x_k(7, 0), + (Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)), + 1.0; x_priori = (xdot * dt) + x_k; setF(dt, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; @@ -313,8 +304,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) /** * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim */ -float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) -{ +float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) { return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); } @@ -332,19 +322,14 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. * The thrust force is then rotated into the body frame using the BodyToGlobal function. */ -void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix &to_modify) -{ +void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix& to_modify) { float interpolatedValue = 0; - if (FSM_state >= STATE_FIRST_BOOST) - { - if (FSM_state < FSMState::STATE_BURNOUT) - { + if (FSM_state >= STATE_FIRST_BOOST) { + if (FSM_state < FSMState::STATE_BURNOUT) { // first stage - if (timestamp >= 0.009) - { + if (timestamp >= 0.009) { auto it = O5500X_data.lower_bound(timestamp); - if (it != O5500X_data.end()) - { + if (it != O5500X_data.end()) { float x0 = it->first; float y0 = it->second; ++it; @@ -353,21 +338,17 @@ void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen:: interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } - } - else - { - if (timestamp >= 0.083) - { - // second stage - auto it = M685W_data.lower_bound(timestamp); - if (it != M685W_data.end()) - { - float x0 = it->first; - float y0 = it->second; - ++it; - float x1 = it->first; - float y1 = it->second; - interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); + } else { + if (timestamp >= 0.083) { + // second stage + auto it = M685W_data.lower_bound(timestamp); + if (it != M685W_data.end()) { + float x0 = it->first; + float y0 = it->second; + ++it; + float x1 = it->first; + float y1 = it->second; + interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); } } } @@ -385,22 +366,17 @@ void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen:: * the new sensor data is. After updating the gain, the state estimate is updated. * */ -void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) -{ - if (FSM_state == FSMState::STATE_IDLE) - { +void EKF::update(BarometerData barometer, Acceleration acceleration, OrientationData orientation, FSMState FSM_state) { + if (FSM_state == FSMState::STATE_IDLE) { float sum = 0; float data[10]; alt_buffer.readSlice(data, 0, 10); - for (float i : data) - { + for (float i : data) { sum += i; } - KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; + KalmanState kalman_state = (KalmanState) {sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; setState(kalman_state); - } - else if (FSM_state >= FSMState::STATE_APOGEE) - { + } else if (FSM_state >= FSMState::STATE_APOGEE) { H(1, 2) = 0; } @@ -416,7 +392,7 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori (accel)(1, 0) = acceleration.ay - 0.065; (accel)(2, 0) = -acceleration.ax - 0.06; - euler_t angles = orientation.getEuler(); + euler_t angles = orientation.euler; angles.yaw = -angles.yaw; Eigen::Matrix acc; @@ -444,9 +420,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori kalman_state.state_est_vel_z = x_k(7, 0); kalman_state.state_est_accel_z = x_k(8, 0); - state.position = (Position){kalman_state.state_est_pos_x, kalman_state.state_est_pos_y, kalman_state.state_est_pos_z}; - state.velocity = (Velocity){kalman_state.state_est_vel_x, kalman_state.state_est_vel_y, kalman_state.state_est_vel_z}; - state.acceleration = (Acceleration){kalman_state.state_est_accel_x, kalman_state.state_est_accel_y, kalman_state.state_est_accel_z}; + state.position = (Position) {kalman_state.state_est_pos_x, kalman_state.state_est_pos_y, + kalman_state.state_est_pos_z}; + state.velocity = (Velocity) {kalman_state.state_est_vel_x, kalman_state.state_est_vel_y, + kalman_state.state_est_vel_z}; + state.acceleration = (Acceleration) {kalman_state.state_est_accel_x, kalman_state.state_est_accel_y, + kalman_state.state_est_accel_z}; } /** @@ -459,17 +438,14 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori * @param &orientation Current orientation * @param current_state Current FSM_state */ -void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) -{ - if (FSM_state >= FSMState::STATE_IDLE) - { - if (FSM_state != last_fsm) - { +void EKF::tick(float dt, float sd, BarometerData& barometer, Acceleration acceleration, OrientationData& orientation, FSMState FSM_state) { + if (FSM_state >= FSMState::STATE_IDLE) { + if (FSM_state != last_fsm) { stage_timestamp = 0; last_fsm = FSM_state; } stage_timestamp += dt; - setF(dt, orientation.roll, orientation.pitch, orientation.yaw); + setF(dt, orientation.euler.roll, orientation.euler.pitch, orientation.euler.yaw); setQ(dt, sd); priori(dt, orientation, FSM_state); update(barometer, acceleration, orientation, FSM_state); @@ -481,8 +457,7 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati * * @return the current state, see sensor_data.h for KalmanData */ -KalmanData EKF::getState() -{ +KalmanData EKF::getState() { return state; } @@ -491,8 +466,7 @@ KalmanData EKF::getState() * * @param state Wanted state vector */ -void EKF::setState(KalmanState state) -{ +void EKF::setState(KalmanState state) { this->state.position.px = state.state_est_pos_x; this->state.position.py = state.state_est_pos_y; this->state.position.pz = state.state_est_pos_z; @@ -514,8 +488,7 @@ void EKF::setState(KalmanState state) * updated based on the time taken per cycle of the Kalman Filter Thread. */ void EKF:: -setQ(float dt, float sd) -{ +setQ(float dt, float sd) { Q(0, 0) = pow(dt, 5) / 20; Q(0, 1) = pow(dt, 4) / 8; Q(0, 2) = pow(dt, 3) / 6; @@ -555,8 +528,7 @@ setQ(float dt, float sd) * @param body_vect Vector for rotation in the body frame * @return Eigen::Matrix Rotated vector in the global frame */ -void EKF::BodyToGlobal(euler_t angles, Eigen::Matrix &body_vect,Eigen::Matrix &to_modify ) -{ +void EKF::BodyToGlobal(euler_t angles, Eigen::Matrix& body_vect, Eigen::Matrix& to_modify) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); @@ -566,11 +538,10 @@ void EKF::BodyToGlobal(euler_t angles, Eigen::Matrix &body_vect,Eig } - /** * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` */ -void EKF::priori() {}; +void EKF::priori() {} /** * @brief Converts a vector in the global frame to the body frame @@ -581,8 +552,7 @@ void EKF::priori() {}; * @return Eigen::Matrix Rotated vector in the body frame * */ -void EKF::GlobalToBody(euler_t angles, Eigen::Matrix &to_modify) -{ +void EKF::GlobalToBody(euler_t angles, Eigen::Matrix& to_modify) { Eigen::Matrix3f roll; roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); Eigen::Matrix3f pitch; @@ -602,8 +572,7 @@ void EKF::GlobalToBody(euler_t angles, Eigen::Matrix &to_modify) * by how the states change over time and also depends on the * current state of the rocket. */ -void EKF::setF(float dt, float wx, float wy, float wz) -{ +void EKF::setF(float dt, float wx, float wy, float wz) { Eigen::Matrix w = Eigen::Matrix::Zero(); w(0, 0) = wx; w(1, 0) = wy; @@ -618,5 +587,3 @@ void EKF::setF(float dt, float wx, float wy, float wz) F_mat(7, 1) = w(1, 0); F_mat(7, 4) = -w(0, 0); } - -EKF ekf; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 235dfdf6..8570684b 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -12,10 +12,10 @@ class EKF : public KalmanFilter { public: EKF(); - void initialize(RocketSystems* args) override; - void priori(); - void priori(float dt, Orientation &orientation, FSMState fsm); - void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; + void initialize(RocketData& data) override; + void priori() override; + void priori(float dt, OrientationData &orientation, FSMState fsm); + void update(BarometerData barometer, Acceleration acceleration, OrientationData orientation, FSMState state) override; void setQ(float dt, float sd); void setF(float dt, float w_x, float w_y, float w_z); @@ -28,7 +28,7 @@ class EKF : public KalmanFilter float linearInterpolation(float x0, float y0, float x1, float y1, float x); - void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); + void tick(float dt, float sd, BarometerData &barometer, Acceleration acceleration, OrientationData &orientation, FSMState state); bool should_reinit = false; private: @@ -47,5 +47,3 @@ class EKF : public KalmanFilter Buffer alt_buffer; KalmanData state; }; - -extern EKF ekf; diff --git a/MIDAS/src/gnc/example_kf.cpp b/MIDAS/src/gnc/example_kf.cpp deleted file mode 100644 index ccfa6775..00000000 --- a/MIDAS/src/gnc/example_kf.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "example_kf.h" - -ExampleKalmanFilter::ExampleKalmanFilter() : KalmanFilter() {} - -void ExampleKalmanFilter::initialize(RocketSystems* args) {} - - -void ExampleKalmanFilter::priori() { - x_priori = (F_mat * x_k); - P_priori = (F_mat * P_k * F_mat.transpose()) + Q; -} - -void ExampleKalmanFilter::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_stat) {} - -void ExampleKalmanFilter::setQ(float dt, float sd) {} - -void ExampleKalmanFilter::setF(float dt) {} - -KalmanData ExampleKalmanFilter::getState() { return KalmanData(); } - -void ExampleKalmanFilter::setState(KalmanState state) -{ - this->state.position.px = state.state_est_pos_x; - this->state.position.py = state.state_est_pos_y; - this->state.position.pz = state.state_est_pos_z; - this->state.acceleration.ax = state.state_est_accel_x; - this->state.acceleration.ay = state.state_est_accel_y; - this->state.acceleration.az = state.state_est_accel_z; - this->state.velocity.vx =state.state_est_vel_x; - this->state.velocity.vy =state.state_est_vel_y; - this->state.velocity.vz =state.state_est_vel_z; -} - -ExampleKalmanFilter example_kf; diff --git a/MIDAS/src/gnc/example_kf.h b/MIDAS/src/gnc/example_kf.h deleted file mode 100644 index ba28f69d..00000000 --- a/MIDAS/src/gnc/example_kf.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include "kalman_filter.h" - -// makes a kalman filter with 9 state variables and 3 sensor inputs -class ExampleKalmanFilter : public KalmanFilter<9, 4> -{ -public: - ExampleKalmanFilter(); - - void initialize(RocketSystems* args) override; - //virtual void initialize(Orientation &orientation, Barometer &barometer, Acceleration &Acceleration); - void priori() override; - void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) override; - - KalmanData getState() override; - void setState(KalmanState state) override; - - void setQ(float dt, float sd); - void setF(float dt); - - Eigen::Matrix bodyToGlobal(euler_t angles, Eigen::Matrix x_k); - -private: - KalmanData state; - KalmanState kalman_state; -}; - -extern ExampleKalmanFilter example_kf; diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h index 61571599..fb142074 100644 --- a/MIDAS/src/gnc/kalman_filter.h +++ b/MIDAS/src/gnc/kalman_filter.h @@ -7,8 +7,7 @@ #include #include "sensor_data.h" -#include "systems.h" - +#include "rocket_state.h" struct KalmanState { float state_est_pos_x; @@ -22,9 +21,8 @@ struct KalmanState { float state_est_accel_z; }; -template -class KalmanFilter -{ +template +class KalmanFilter { protected: Eigen::Matrix x_k; Eigen::Matrix F_mat; @@ -40,8 +38,7 @@ class KalmanFilter Eigen::Matrix B; public: - KalmanFilter() - { + KalmanFilter() { x_k = Eigen::Matrix::Zero(); F_mat = Eigen::Matrix::Zero(); H = Eigen::Matrix::Zero(); @@ -55,10 +52,10 @@ class KalmanFilter B = Eigen::Matrix::Zero(); } - - virtual void initialize(RocketSystems* args) = 0; + + virtual void initialize(RocketData&) = 0; virtual void priori() = 0; - virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) = 0; + virtual void update(BarometerData, Acceleration, OrientationData, FSMState) = 0; virtual KalmanData getState() = 0; virtual void setState(KalmanState state) = 0; }; diff --git a/MIDAS/src/gnc/yessir.cpp b/MIDAS/src/gnc/yessir.cpp deleted file mode 100644 index 9aef174c..00000000 --- a/MIDAS/src/gnc/yessir.cpp +++ /dev/null @@ -1,325 +0,0 @@ -#include "yessir.h" -#include "finite-state-machines/fsm_states.h" - -Yessir::Yessir() : KalmanFilter() { - state = KalmanData(); -} - -/** - * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms - * apart - * - * The following for loop takes a series of barometer measurements on start - * up and takes the average of them in order to initialize the kalman filter - * to the correct initial barometric altitude. This is done so that the - * kalman filter takes minimal time to converge to an accurate state - * estimate. This process is significantly faster than allowing the state as - * letting the filter to converge to the correct state can take up to 3 min. - * This specific process was used because the barometric altitude will - * change depending on the weather and thus, the initial state estimate - * cannot be hard coded. A GPS altitude may be used instead but due to GPS - * losses during high speed/high altitude flight, it is inadvisable with the - * current hardware to use this as a solution. Reference frames should also - * be kept consistent (do not mix GPS altitude and barometric). - * - */ - -void Yessir::initialize(RocketSystems* args) { - Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); - - float sum = 0; - - for (int i = 0; i < 30; i++) { - Barometer barometer = args->rocket_data.barometer.getRecent(); - LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); - Acceleration accelerations = { - .ax = initial_accelerometer.ax, - .ay = initial_accelerometer.ay, - .az = initial_accelerometer.az - }; - sum += barometer.altitude; - - init_accel(0, 0) += accelerations.az; - init_accel(1, 0) += accelerations.ay; - init_accel(2, 0) += -accelerations.ax; - THREAD_SLEEP(100); - } - - init_accel(0, 0) /= 30; - init_accel(1, 0) /= 30; - init_accel(2, 0) /= 30; - - euler_t euler = orientation.getEuler(); - euler.yaw = -euler.yaw; - world_accel = BodyToGlobal(euler, init_accel); - - // set x_k - x_k(0, 0) = sum / 30; - x_k(3, 0) = 0; - x_k(6, 0) = 0; - - // set F - for (int i = 0; i < 3; i++) { - F_mat(3 * i, 3 * i + 1) = s_dt; - F_mat(3 * i, 3 * i + 2) = (s_dt * s_dt) / 2; - F_mat(3 * i + 1, 3 * i + 2) = s_dt; - - F_mat(3 * i, 3 * i) = 1; - F_mat(3 * i + 1, 3 * i + 1) = 1; - F_mat(3 * i + 2, 3 * i + 2) = 1; - } - - Q(0, 0) = pow(s_dt, 5) / 20; - Q(0, 1) = pow(s_dt, 4) / 8; - Q(0, 2) = pow(s_dt, 3) / 6; - Q(1, 1) = pow(s_dt, 3) / 8; - Q(1, 2) = pow(s_dt, 2) / 2; - Q(2, 2) = s_dt; - Q(1, 0) = Q(0, 1); - Q(2, 0) = Q(0, 2); - Q(2, 1) = Q(1, 2); - - Q(3, 3) = pow(s_dt, 5) / 20; - Q(3, 4) = pow(s_dt, 4) / 8; - Q(3, 5) = pow(s_dt, 3) / 6; - Q(4, 4) = pow(s_dt, 3) / 8; - Q(4, 5) = pow(s_dt, 2) / 2; - Q(5, 5) = s_dt; - Q(4, 3) = Q(3, 4); - Q(5, 3) = Q(3, 5); - Q(5, 4) = Q(4, 5); - - Q(6, 6) = pow(s_dt, 5) / 20; - Q(6, 7) = pow(s_dt, 4) / 8; - Q(6, 8) = pow(s_dt, 3) / 6; - Q(7, 7) = pow(s_dt, 3) / 8; - Q(7, 8) = pow(s_dt, 2) / 2; - Q(8, 8) = s_dt; - Q(7, 6) = Q(6, 7); - Q(8, 6) = Q(6, 8); - Q(8, 7) = Q(7, 8); - - // set H - H(0, 0) = 1; - H(1, 2) = 1; - H(2, 5) = 1; - H(3, 8) = 1; - - Q = Q * spectral_density_; - - // set R - R(0, 0) = 2.0; - R(1, 1) = 1.9; - R(2, 2) = 10; - R(3, 3) = 10; - - // set B (don't care about what's in B since we have no control input) - B(2, 0) = -1; -} - -/** - * @brief Estimates current state of the rocket without current sensor data - * - * The priori step of the Kalman filter is used to estimate the current state - * of the rocket without knowledge of the current sensor data. In other words, - * it extrapolates the state at time n+1 based on the state at time n. - */ -void Yessir::priori() { - // x_priori = (F @ x_k) + ((B @ u).T) #* For some reason doesnt work when B - // or u is = 0 - x_priori = (F_mat * x_k); - P_priori = (F_mat * P_k * F_mat.transpose()) + Q; -} - -/** - * @brief Update Kalman Gain and state estimate with current sensor data - * - * After receiving new sensor data, the Kalman filter updates the state estimate - * and Kalman gain. The Kalman gain can be considered as a measure of how uncertain - * the new sensor data is. After updating the gain, the state estimate is updated. - * - */ -void Yessir::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { - if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { - float sum = 0; - float data[10]; - alt_buffer.readSlice(data, 0, 10); - for (float i : data) { - sum += i; - } - KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; - setState(kalman_state); - } else if (FSM_state >= FSMState::STATE_APOGEE) { - H(1, 2) = 0; - } - - Eigen::Matrix S_k = Eigen::Matrix::Zero(); - S_k = (((H * P_priori * H.transpose()) + R)).inverse(); - Eigen::Matrix identity = Eigen::Matrix::Identity(); - K = (P_priori * H.transpose()) * S_k; - - // Sensor Measurements - Eigen::Matrix accel = Eigen::Matrix::Zero(); - - accel(0, 0) = acceleration.az - 0.045; - accel(1, 0) = acceleration.ay - 0.065; - accel(2, 0) = -acceleration.ax - 0.06; - - euler_t angles = orientation.getEuler(); - angles.yaw = -angles.yaw; - - Eigen::Matrix acc = BodyToGlobal(angles, accel); - - y_k(1, 0) = (acc(0)) * 9.81 - 9.81; - y_k(2, 0) = (acc(1)) * 9.81; - y_k(3, 0) = (acc(2)) * 9.81; - - y_k(0, 0) = barometer.altitude; - alt_buffer.push(barometer.altitude); - - - // # Posteriori Update - x_k = x_priori + K * (y_k - (H * x_priori)); - P_k = (identity - K * H) * P_priori; - // Joseph (Expanded) Form - // P_k = (identity - K * H) * P_priori * (identity - K * H).transpose() + K * R * K.transpose(); - - kalman_state.state_est_pos_x = x_k(0, 0); - kalman_state.state_est_vel_x = x_k(1, 0); - kalman_state.state_est_accel_x = x_k(2, 0); - kalman_state.state_est_pos_y = x_k(3, 0); - kalman_state.state_est_vel_y = x_k(4, 0); - kalman_state.state_est_accel_y = x_k(5, 0); - kalman_state.state_est_pos_z = x_k(6, 0); - kalman_state.state_est_vel_z = x_k(7, 0); - kalman_state.state_est_accel_z = x_k(8, 0); - - state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z}; - state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z}; - state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z}; -} - -/** - * @brief Run Kalman filter calculations as long as FSM has passed IDLE - * - * @param dt Time step calculated by the Kalman Filter Thread - * @param sd Spectral density of the noise - * @param &barometer Data of the current barometer - * @param acceleration Current acceleration - * @param &orientation Current orientation - * @param current_state Current FSM_state - */ -void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { - if (FSM_state >= FSMState::STATE_IDLE) { - setF(dt / 1000); - setQ(dt / 1000, sd); - priori(); - update(barometer, acceleration, orientation, FSM_state); - } -} - -/** - * @brief Converts a vector in the body frame to the global frame - * - * @param angles Roll, pitch, yaw angles - * @param body_vect Vector for rotation in the body frame - * @return Eigen::Matrix Rotated vector in the global frame - */ -Eigen::Matrix Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix body_vect) { - Eigen::Matrix3f roll, pitch, yaw; - roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - return yaw * pitch * roll * body_vect; -} - - -/** - * @brief Getter for state X - * - * @return the current state, see sensor_data.h for KalmanData - */ -KalmanData Yessir::getState() { - return state; -} - -/** - * @brief Sets state vector x - * - * @param state Wanted state vector - */ -void Yessir::setState(KalmanState state) { - this->state.position.px = state.state_est_pos_x; - this->state.position.py = state.state_est_pos_y; - this->state.position.pz = state.state_est_pos_z; - this->state.acceleration.ax = state.state_est_accel_x; - this->state.acceleration.ay = state.state_est_accel_y; - this->state.acceleration.az = state.state_est_accel_z; - this->state.velocity.vx = state.state_est_vel_x; - this->state.velocity.vy = state.state_est_vel_y; - this->state.velocity.vz = state.state_est_vel_z; -} - -/** - * @brief Sets the Q matrix given time step and spectral density. - * - * @param dt Time step calculated by the Kalman Filter Thread - * @param sd Spectral density of the noise - * - * The Q matrix is the covariance matrix for the process noise and is - * updated based on the time taken per cycle of the Kalman Filter Thread. - */ -void Yessir::setQ(float dt, float sd) { - Q(0, 0) = pow(dt, 5) / 20; - Q(0, 1) = pow(dt, 4) / 8; - Q(0, 2) = pow(dt, 3) / 6; - Q(1, 1) = pow(dt, 3) / 8; - Q(1, 2) = pow(dt, 2) / 2; - Q(2, 2) = dt; - Q(1, 0) = Q(0, 1); - Q(2, 0) = Q(0, 2); - Q(2, 1) = Q(1, 2); - Q(3, 3) = pow(dt, 5) / 20; - Q(3, 4) = pow(dt, 4) / 8; - Q(3, 5) = pow(dt, 3) / 6; - Q(4, 4) = pow(dt, 3) / 8; - Q(4, 5) = pow(dt, 2) / 2; - Q(5, 5) = dt; - Q(4, 3) = Q(3, 4); - Q(5, 3) = Q(3, 5); - Q(5, 4) = Q(4, 5); - - Q(6, 6) = pow(dt, 5) / 20; - Q(6, 7) = pow(dt, 4) / 8; - Q(6, 8) = pow(dt, 3) / 6; - Q(7, 7) = pow(dt, 3) / 8; - Q(7, 8) = pow(dt, 2) / 2; - Q(8, 8) = dt; - Q(7, 6) = Q(6, 7); - Q(8, 6) = Q(6, 8); - Q(8, 7) = Q(7, 8); - - Q *= sd; -} - -/** - * @brief Sets the F matrix given time step. - * - * @param dt Time step calculated by the Kalman Filter Thread - * - * The F matrix is the state transition matrix and is defined - * by how the states change over time. - */ -void Yessir::setF(float dt) { - for (int i = 0; i < 3; i++) { - F_mat(3 * i, 3 * i + 1) = s_dt; - F_mat(3 * i, 3 * i + 2) = (dt * s_dt) / 2; - F_mat(3 * i + 1, 3 * i + 2) = s_dt; - - F_mat(3 * i, 3 * i) = 1; - F_mat(3 * i + 1, 3 * i + 1) = 1; - F_mat(3 * i + 2, 3 * i + 2) = 1; - } -} - -Yessir yessir; diff --git a/MIDAS/src/gnc/yessir.h b/MIDAS/src/gnc/yessir.h deleted file mode 100644 index 798b9738..00000000 --- a/MIDAS/src/gnc/yessir.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include "kalman_filter.h" -#include "sensor_data.h" -#include "Buffer.h" - -#define NUM_STATES 9 -#define NUM_SENSOR_INPUTS 4 -#define ALTITUDE_BUFFER_SIZE 10 - -class Yessir : public KalmanFilter -{ -public: - Yessir(); - void initialize(RocketSystems* args) override; - void priori() override; - void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; - - void setQ(float dt, float sd); - void setF(float dt); - Eigen::Matrix BodyToGlobal(euler_t angles, Eigen::Matrix x_k); - - KalmanData getState() override; - void setState(KalmanState state) override; - - void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); - -private: - float s_dt = 0.05f; - float spectral_density_ = 13.0f; - float kalman_apo = 0; - KalmanState kalman_state; - - Eigen::Matrix init_accel = Eigen::Matrix::Zero(); - Eigen::Matrix world_accel; - Buffer alt_buffer; - KalmanData state; -}; - -extern Yessir yessir; diff --git a/MIDAS/src/hal.h b/MIDAS/src/hal.h index 8da2342f..f6961cb8 100644 --- a/MIDAS/src/hal.h +++ b/MIDAS/src/hal.h @@ -4,65 +4,81 @@ #ifdef SILSIM #include "silsim/emulation.h" #else +#include "FreeRTOSConfig.h" + #include +#include +#include +#include #endif -#include "FreeRTOSConfig.h" - -#include "Mutex.h" -#include "Queue.h" +#include "sensor_data.h" +#include "errors.h" /** - * The size of a thread stack, in bytes. - */ -#define STACK_SIZE 8192 + * @brief Delays the running thread. + * @param millis The time to delay in milliseconds. +*/ +#define THREAD_SLEEP(millis) vTaskDelay(pdMS_TO_TICKS(millis)) -/** - * The ESP32 has two physical cores, which will each be dedicated to one group of threads. - * The SENSOR_CORE runs the threads which write to the sensor_data struct (mostly sensor polling threads). - */ -#define SENSOR_CORE ((BaseType_t) 0) +enum class LED { + BLUE = 0, + RED = 1, + ORANGE = 2, + GREEN = 3 +}; -/** - * The ESP32 has two physical cores, which will each be dedicated to one group of threads. - * The DATA_CORE runs the GPS thread, as well as the threads which read from the sensor_data struct (e.g. SD logging). - */ -#define DATA_CORE ((BaseType_t) 1) +enum class Channel { + A = 0, + B = 1, + C = 2, + D = 3 +}; -/** - * Macro for declaring a thread. Creates a function with the name suffixed with `_thread`, annotated with [[noreturn]]. - * - * @param name The name of the task. - * @param param The single parameter to input into the thread. - */ -#define DECLARE_THREAD(name, param) [[noreturn]] void name##_thread(param) +enum class Camera { + Side = 0, + Bulkhead = 1 +}; /** - * Macro for creating and starting a thread declared with `DECLARE_THREAD`. This is a statement and has no return value. - * Never put this in a scope that will end before the thread should. + * @struct Sensors * - * @param name Name of the thread. - * @param core Core for the task to be pinned to, either `SENSOR_CORE` or `DATA_CORE`. - * @param arg Argument passed in to the `param` argument of `DECLARE_THREAD`. - * @param prio Priority of the thread. - */ -#define START_THREAD(name, core, arg, prio) StaticTask_t name##_task; \ - static unsigned char name##_stack[STACK_SIZE]; \ - xTaskCreateStaticPinnedToCore(((TaskFunction_t) name##_thread), #name, STACK_SIZE, arg, tskIDLE_PRIORITY + prio, name##_stack, &name##_task, core) -/* - * Parameters for xTaskCreateStaticPinnedToCore are as follows in parameter order: - * - Function to be run by the thread, this contains a `while(true)` loop - * - Name of thread - * - Size of the stack for each thread in words (1 word = 4 bytes) - * - Arguments to be passed into the function, this will generally eb the config file - * - Priority of the task, in allmost all cases, this will be the idle priority plus one - * - The actual stack memory to use - * - A handle to reference the task with - * - The core to pin the task to + * @brief holds all interfaces for all sensors on MIDAS */ +template +struct HwInterface { + ErrorCode init_all() { return static_cast(this)->init_all(); }; -/** - * @brief Delays the running thread. - * @param millis The time to delay in milliseconds. -*/ -#define THREAD_SLEEP(millis) vTaskDelay(pdMS_TO_TICKS(millis)) + LowGData read_low_g() { return static_cast(this)->read_low_g(); } + LowGLSMData read_low_g_lsm() { return static_cast(this)->read_low_g_lsm(); } + HighGData read_high_g() { return static_cast(this)->read_high_g(); } + BarometerData read_barometer() { return static_cast(this)->read_barometer(); } + ContinuityData read_continuity() { return static_cast(this)->read_continuity(); } + VoltageData read_voltage() { return static_cast(this)->read_voltage(); } + bool is_orientation_ready() { return static_cast(this)->is_orientation_ready(); } + OrientationData read_orientation() { return static_cast(this)->read_orientation(); } + MagnetometerData read_magnetometer() { return static_cast(this)->read_magnetometer(); } + bool is_gps_ready() { return static_cast(this)->is_gps_ready(); } + GPSData read_gps() { return static_cast(this)->read_gps(); } + + void set_led(LED which, bool value) { return static_cast(this)->set_led(which, value); } + + void transmit_bytes(uint8_t* memory, size_t count) { return static_cast(this)->transmit_bytes(memory, count); } + bool receive_bytes(uint8_t* memory, size_t count, int wait_milliseconds) { return static_cast(this)->receive_bytes(memory, count, wait_milliseconds); } + + template + void transmit(T* value) { return transmit_bytes((uint8_t*) value, sizeof(T)); } + template + bool receive(T* value, int wait_milliseconds) { return receive_bytes((uint8_t*) value, sizeof(T), wait_milliseconds); } + + void set_global_arm(bool to_high) { return static_cast(this)->set_global_arm(to_high); } + void set_pin_firing(Channel which, bool to_high) { return static_cast(this)->set_pin_firing(which, to_high); } + + void set_camera_on(Camera which, bool on) { return static_cast(this)->set_camera_on(which, on); } + void set_camera_source(Camera which) { return static_cast(this)->set_camera_source(which); } + void set_video_transmit(bool on) { return static_cast(this)->set_video_transmit(on); } + uint8_t get_camera_state() { return static_cast(this)->get_camera_state(); } + +protected: + HwInterface() = default; +}; diff --git a/MIDAS/src/hardware/Barometer.cpp b/MIDAS/src/hardware/Barometer.cpp deleted file mode 100644 index 8eaf9cef..00000000 --- a/MIDAS/src/hardware/Barometer.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "sensors.h" -#include - -MS5611 MS(MS5611_CS); //singleton object for the MS sensor - -/** - * @brief Initializes barometer, returns NoError - * - * @return Error code -*/ -ErrorCode BarometerSensor::init() { - MS.init(); - - return ErrorCode::NoError; -} - -/** - * @brief Reads the pressure and temperature from the MS5611 - * - * @return Barometer data packet -*/ -Barometer BarometerSensor::read() { - MS.read(12); - - /* - * TODO: Switch to latest version of library (0.3.9) when we get hardware to verify - * Equation derived from https://en.wikipedia.org/wiki/Atmospheric_pressure#Altitude_variation - */ - float pressure = static_cast(MS.getPressure() * 0.01 + 26.03); // getPressure is in milibars so it's milibars * 0.01? - float temperature = static_cast(MS.getTemperature() * 0.01); // Celcius - float altitude = static_cast(-log(pressure * 0.000987) * (temperature + 273.15) * 29.254); - - return Barometer(temperature, pressure, altitude); -} diff --git a/MIDAS/src/hardware/Continuity.cpp b/MIDAS/src/hardware/Continuity.cpp deleted file mode 100644 index c5cd3a2b..00000000 --- a/MIDAS/src/hardware/Continuity.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "sensors.h" -#include "ads7138-q1.h" -#include -#include - -#define PYRO_VOLTAGE_DIVIDER (5.0 / (5.0 + 20.0)) //voltage divider for pyro batt voltage, check hardware schematic -#define CONT_VOLTAGE_DIVIDER (5.0 / (5.0 + 20.0)) //voltage divider for continuity voltage, check hardware schematic - -// Reads the given register from the pyro power monitor -int read_pwr_monitor_register(int reg, int bytes) { - Wire1.beginTransmission(0x41); // I2C Address 0x41 is pyro pwr monitor - Wire1.write(reg); - if(Wire1.endTransmission()){ - Serial.println("I2C Error"); - } - - Wire1.requestFrom(0x41, bytes); - int val = 0; - - for(int i = 0; i < bytes; i++){ - int v = Wire1.read(); - if(v == -1) Serial.println("I2C Read Error"); - val = (val << 8) | v; - } - - return val; -} - -/** - * @brief Initializes ADC, returns NoError - * - * @return Error code -*/ -ErrorCode ContinuitySensor::init() { - // ADS7138Init(); // Ask ADS to init the pins, we still need to get the device to actually read - // Configure the INA745b MODE:ContTCV VBUS:1052us VSEN:1052us TCT:1052us AVG:128 - constexpr uint16_t INA_config = (0xF << 12) | (0x5 << 9) | (0x5 << 6) | (0x5 << 3) | (0x4); - - Wire1.beginTransmission(0x41); - Wire1.write(0x1); - - Wire1.write(((INA_config >> 8) & 0xFF)); - Wire1.write(((INA_config >> 0) & 0xFF)); - - if(Wire1.endTransmission()){ - Serial.println("Pyro PWR I2C Error"); - return ErrorCode::ContinuityCouldNotBeInitialized; - } - - Serial.println("Pyro PWR monitor configured"); - - return ErrorCode::NoError; -} - -/** - * @brief Reads the value of the ADC - * - * @return Continuity data packet -*/ -Continuity ContinuitySensor::read() { - Continuity continuity; - //ADC reference voltage is 3.3, returns 12 bit value - - // MIDAS 2.1 rev A ADC sense fix: - int16_t current = read_pwr_monitor_register(0x7, 2); - int voltage = read_pwr_monitor_register(0x5, 2); - - float voltage_normalized = voltage * 3.125 / 1000.0; // V - - float continuous_channels = 0.0; - float absolute_current = 0.0; - float expected_current = 0.0; - - if(voltage_normalized > 1) { - absolute_current = current * 1.2 / 1000.0; - - expected_current = (voltage_normalized - 0.2) / 470.0; // Account for diode voltage drop - continuous_channels = absolute_current / expected_current; - } - - // We don't have the granularity to determine individual voltages, so all of them will give the # of continuous channels - continuity.pins[0] = continuous_channels; // Number of continuous channels on the pyro bus - continuity.pins[1] = absolute_current; // The absolute current running through the pyro bus - continuity.pins[2] = expected_current; // Calculated expected current based on current pyro bus voltage - continuity.pins[3] = voltage_normalized; // Pyro bus voltage - - // Serial.println(continuous_channels); - return continuity; -} diff --git a/MIDAS/src/hardware/E22.cpp b/MIDAS/src/hardware/E22.cpp index 0d4ccacf..2e6991a3 100644 --- a/MIDAS/src/hardware/E22.cpp +++ b/MIDAS/src/hardware/E22.cpp @@ -1,4 +1,4 @@ -#include"E22.h" +#include "E22.h" #define DBG_PRINT(x) (void) 0 #define SX1268Check(x) if((x) == SX1268Error::BusyTimeout) { return SX1268Error::BusyTimeout; } @@ -452,4 +452,3 @@ SX1268Error SX1268::check_device_errors() { return SX1268Error::NoError; } - diff --git a/MIDAS/src/hardware/E22.h b/MIDAS/src/hardware/E22.h index 49ed376a..14a587ca 100644 --- a/MIDAS/src/hardware/E22.h +++ b/MIDAS/src/hardware/E22.h @@ -1,8 +1,7 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include /*! @@ -263,4 +262,4 @@ class SX1268 { int prev_rx_error; bool busy_fault; SPISettings spiSettings = SPISettings(10000000, MSBFIRST, SPI_MODE0); -}; \ No newline at end of file +}; diff --git a/MIDAS/src/hardware/GPSSensor.cpp b/MIDAS/src/hardware/GPSSensor.cpp deleted file mode 100644 index 5656636b..00000000 --- a/MIDAS/src/hardware/GPSSensor.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include - -#include - -#include "pins.h" -#include "sensors.h" -#include "sensor_data.h" - -SFE_UBLOX_GNSS ublox; - -/** - * @brief Initializes GPS, returns NoError - * - * @return Error code - */ -ErrorCode GPSSensor::init() { - if (!ublox.begin()) { - return ErrorCode::GPSCouldNotBeInitialized; - } - - ublox.setDynamicModel(DYN_MODEL_AIRBORNE4g); - ublox.setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA); //Set the I2C port to output both NMEA and UBX messages - // Set the measurment rate faster than one HZ if necessary - // ublox.setMeasurementRate(100); - ublox.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR - - //This will pipe all NMEA sentences to the serial port so we can see them - - return ErrorCode::NoError; -} - - -/** - * @brief Reads the GPS data from the sensor (lat, long, altitude, sat count, etc) - * - * @return GPS data packet - */ -GPS GPSSensor::read() { - return GPS{ublox.getLatitude(), ublox.getLongitude(), (float) ublox.getAltitude() / 1000.f, (float) ublox.getGroundSpeed() / 1000.f, ublox.getFixType(), ublox.getUnixEpoch()}; -} - -bool GPSSensor::valid() { - return ublox.getPVT(); -} diff --git a/MIDAS/src/hardware/HighG.cpp b/MIDAS/src/hardware/HighG.cpp deleted file mode 100644 index 1123f9bd..00000000 --- a/MIDAS/src/hardware/HighG.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "sensors.h" -#include "SparkFun_Qwiic_KX13X.h" - -QwiicKX134 KX; // global static instance of the sensor - -/** - * @brief Initializes the high G sensor - * - * @return Error Code -*/ -ErrorCode HighGSensor::init() { - KX.beginSPI(KX134_CS); - if (!KX.initialize(DEFAULT_SETTINGS)) { - return ErrorCode::HighGCouldNotBeInitialized; - } - - if(!KX.setOutputDataRate(0xb)) { - return ErrorCode::HighGCouldNotUpdateDataRate; - } - - KX.setRange(3); - return ErrorCode::NoError; -} - -/** - * @brief Reads and returns the data from the sensor - * - * @return a HighGData packet with current acceleration in all three axes -*/ -HighGData HighGSensor::read() { - auto data = KX.getAccelData(); - return HighGData(data.xData, data.yData, data.zData); -} diff --git a/MIDAS/src/hardware/LowG.cpp b/MIDAS/src/hardware/LowG.cpp deleted file mode 100644 index e74c2290..00000000 --- a/MIDAS/src/hardware/LowG.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "sensors.h" -#include "PL_ADXL355.h" - -PL::ADXL355 sensor(ADXL355_CS); //singleton object for the adxl - -/** - * @brief Initializes the low G sensor - * - * @return Error Code -*/ -ErrorCode LowGSensor::init() { - ErrorCode error = ErrorCode::NoError; - sensor.begin(); - sensor.setRange(PL::ADXL355_Range::range2g); - sensor.setOutputDataRate(PL::ADXL355_OutputDataRate::odr1000); - // todo set low pass filter frequency to 250hx - sensor.enableMeasurement(); - return error; -} - -/** - * @brief Reads and returns the data from the sensor - * - * @return a LowGData packet with current acceleration in all three axes -*/ -LowGData LowGSensor::read() -{ - auto data = sensor.getAccelerations(); - - return { data.x, data.y, data.z }; -} diff --git a/MIDAS/src/hardware/LowGLSM.cpp b/MIDAS/src/hardware/LowGLSM.cpp deleted file mode 100644 index 951be540..00000000 --- a/MIDAS/src/hardware/LowGLSM.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "sensors.h" -#include - -LSM6DS3Class LSM(SPI, LSM6DS3_CS, 46); // global static instance of the sensor - -/** - * @brief Initializes the low G LSM sensor - * - * @return Error Code -*/ -ErrorCode LowGLSMSensor::init() { - if (!LSM.begin()) { - return ErrorCode::GyroCouldNotBeInitialized; - } - return ErrorCode::NoError; -} - -/** - * @brief Reads and returns the data from the sensor - * - * @return a LowGLSM packet with current acceleration and gyro in all three axes -*/ -LowGLSM LowGLSMSensor::read() { - // read from aforementioned global instance of sensor - LowGLSM result; - LSM.readAcceleration(result.ax, result.ay, result.az); - LSM.readGyroscope(result.gx, result.gy, result.gz); - return result; -} diff --git a/MIDAS/src/hardware/Magnetometer.cpp b/MIDAS/src/hardware/Magnetometer.cpp deleted file mode 100644 index 23ef322d..00000000 --- a/MIDAS/src/hardware/Magnetometer.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include - -#include "sensors.h" -#include "hal.h" - -Adafruit_LIS3MDL LIS3MDL; // global static instance of the sensor - -ErrorCode MagnetometerSensor::init() { - if (!LIS3MDL.begin_SPI(LIS3MDL_CS)) { // Checks if sensor is connected - return ErrorCode::MagnetometerCouldNotBeInitialized; - } - LIS3MDL.setOperationMode(LIS3MDL_CONTINUOUSMODE); // Reading continuously, instead of single-shot or off - LIS3MDL.setDataRate(LIS3MDL_DATARATE_155_HZ); - LIS3MDL.setRange(LIS3MDL_RANGE_4_GAUSS); // Earth's magnetic field is 1/2 gauss, can detect high current - return ErrorCode::NoError; -} - -Magnetometer MagnetometerSensor::read() { - // read from aforementioned global instance of sensor - LIS3MDL.read(); - - float mx = LIS3MDL.x_gauss; - float my = LIS3MDL.y_gauss; - float mz = LIS3MDL.z_gauss; - Magnetometer reading{mx, my, mz}; - return reading; -} diff --git a/MIDAS/src/hardware/Orientation.cpp b/MIDAS/src/hardware/Orientation.cpp index 3f8f6b93..747c87d6 100644 --- a/MIDAS/src/hardware/Orientation.cpp +++ b/MIDAS/src/hardware/Orientation.cpp @@ -1,8 +1,10 @@ +#include + #include +#include + #include "sensors.h" -#include "Adafruit_BNO08x.h" -#include "sensor_data.h" -#include +#include "pins.h" // global static instance of the sensor Adafruit_BNO08x imu(BNO086_RESET); @@ -11,38 +13,34 @@ Adafruit_BNO08x imu(BNO086_RESET); #define REPORT_INTERVAL_US_AV 100000 unsigned long lastTime = 0; float deltaTime = 0; +sh2_SensorValue_t event; /** * @brief Initializes the bno sensor * * @return Error Code */ -ErrorCode OrientationSensor::init() -{ +ErrorCode init_orientation() { digitalWrite(BNO086_RESET, OUTPUT); delay(100); // do whatever steps to initialize the sensor // if it errors, return the relevant error code - if (!imu.begin_SPI(BNO086_CS, BNO086_INT)) - { + if (!imu.begin_SPI(BNO086_CS, BNO086_INT)) { return ErrorCode::CannotConnectBNO; } Serial.println("Setting BNO desired reports"); - if (!imu.enableReport(SH2_ARVR_STABILIZED_RV, REPORT_INTERVAL_US)) - { + if (!imu.enableReport(SH2_ARVR_STABILIZED_RV, REPORT_INTERVAL_US)) { return ErrorCode::CannotInitBNO; } - if (!imu.enableReport(SH2_GYRO_INTEGRATED_RV, REPORT_INTERVAL_US_AV)) - { + if (!imu.enableReport(SH2_GYRO_INTEGRATED_RV, REPORT_INTERVAL_US_AV)) { return ErrorCode::CannotInitBNO; } return ErrorCode::NoError; } -float angle_between_quaternions(const Quaternion &q1, const Quaternion &q2) -{ +float angle_between_quaternions(const Quaternion& q1, const Quaternion& q2) { float dot_product = Quaternion::dot(q1, q2); return 2.0f * std::acos(std::fabs(dot_product)); } @@ -58,8 +56,7 @@ float angle_between_quaternions(const Quaternion &q1, const Quaternion &q2) * * @return 3D representation of the quaternion */ -Vec3 quaternionToEuler(float qr, float qi, float qj, float qk, bool degrees) -{ +Vec3 quaternionToEuler(float qr, float qi, float qj, float qk, bool degrees) { float sqr = sq(qr); float sqi = sq(qi); float sqj = sq(qj); @@ -80,8 +77,7 @@ Vec3 quaternionToEuler(float qr, float qi, float qj, float qk, bool degrees) * * @return Euler angle vector representation of the quaternion */ -Vec3 quaternionToEulerRV(sh2_RotationVectorWAcc_t *rotational_vector, bool degrees) -{ +Vec3 quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, bool degrees) { return quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, degrees); } @@ -94,22 +90,19 @@ Vec3 quaternionToEulerRV(sh2_RotationVectorWAcc_t *rotational_vector, bool degre * * @return Euler angle vector representation of the quaternion */ -Vec3 quaternionToEulerGI(sh2_GyroIntegratedRV_t *rotational_vector, bool degrees) -{ +Vec3 quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, bool degrees) { return quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, degrees); } -std::tuple euler_to_vector(float pitch, float yaw) -{ +std::tuple euler_to_vector(float pitch, float yaw) { float x = cos(pitch) * cos(yaw); float y = cos(pitch) * sin(yaw); float z = sin(pitch); return {x, y, z}; } -float angular_difference(float pitch1, float yaw1, float pitch2, float yaw2) -{ +float angular_difference(float pitch1, float yaw1, float pitch2, float yaw2) { auto [x1, y1, z1] = euler_to_vector(pitch1, yaw1); auto [x2, y2, z2] = euler_to_vector(pitch2, yaw2); @@ -129,140 +122,106 @@ Eigen::Matrix3f generate_rotation_matrix(Vec3 rpy_vec) { float pitch = rpy_vec.y; float yaw = rpy_vec.z; - Eigen::Matrix3f Rx { + Eigen::Matrix3f Rx{ {1, 0, 0}, {0, cos(roll), -sin(roll)}, {0, sin(roll), cos(roll)} }; - Eigen::Matrix3f Ry { + Eigen::Matrix3f Ry{ {cos(pitch), 0, sin(pitch)}, {0, 1, 0}, {-sin(pitch), 0, cos(pitch)} }; - Eigen::Matrix3f Rz { + Eigen::Matrix3f Rz{ {cos(yaw), -sin(yaw), 0}, {sin(yaw), cos(yaw), 0}, {0, 0, 1} }; return Rx * Ry * Rz; -} +} /** * @brief Reads and returns the data from the sensor * * @return An orientation packet with orientation, acceleration, gyroscope, and magenetometer for all axes, along with temperature and pressure */ -Orientation OrientationSensor::read() -{ - // read from aforementioned global instance of sensor - sh2_SensorValue_t event; - Vec3 euler; - - Vec3 filtered_euler = {0, 0, 0}; - const float alpha = 0.98; // Higher values dampen out current measurements --> reduce peaks +OrientationData HwImpl::read_orientation() { unsigned long currentTime = millis(); - deltaTime = (currentTime - lastTime) / 1000.0; + deltaTime = (float) (currentTime - lastTime) / 1000.0f; lastTime = currentTime; - if (imu.getSensorEvent(&event)) - { - Orientation sensor_reading; - sensor_reading.has_data = true; + OrientationData sensor_reading; + sensor_reading.has_data = true; - switch (event.sensorId) - { - case SH2_ARVR_STABILIZED_RV: + Vec3 euler = { 0, 0, 0 }; + switch (event.sensorId) { + case SH2_ARVR_STABILIZED_RV: { euler = quaternionToEulerRV(&event.un.arvrStabilizedRV, true); sensor_reading.reading_type = OrientationReadingType::FULL_READING; break; - case SH2_GYRO_INTEGRATED_RV: + } + case SH2_GYRO_INTEGRATED_RV: { sensor_reading.reading_type = OrientationReadingType::ANGULAR_VELOCITY_UPDATE; sensor_reading.angular_velocity.vx = event.un.gyroIntegratedRV.angVelX; sensor_reading.angular_velocity.vy = event.un.gyroIntegratedRV.angVelY; sensor_reading.angular_velocity.vz = event.un.gyroIntegratedRV.angVelZ; - + return sensor_reading; } - - // filtered_euler.x = alpha * (euler.x) + (1 - alpha) * prev_x; - // filtered_euler.y = alpha * (euler.y) + (1 - alpha) * prev_y; - // filtered_euler.z = alpha * (euler.z ) + (1 - alpha) * prev_z; - - // prev_x = euler.x; - // prev_y = euler.y; - // prev_z = euler.z; - - /* - sensor_reading.yaw = -filtered_euler.y; - sensor_reading.pitch = filtered_euler.x; - sensor_reading.roll = filtered_euler.z; - */ - - sensor_reading.yaw = -euler.y; - sensor_reading.pitch = euler.x; - sensor_reading.roll = euler.z; - - sensor_reading.linear_acceleration.ax = -event.un.accelerometer.y; - sensor_reading.linear_acceleration.ay = event.un.accelerometer.x; - sensor_reading.linear_acceleration.az = event.un.accelerometer.z; - - Velocity velocity; - velocity.vx = sensor_reading.linear_acceleration.ax * deltaTime + velocity.vx; - velocity.vy = sensor_reading.linear_acceleration.ay * deltaTime + velocity.vy; - velocity.vz = sensor_reading.linear_acceleration.az * deltaTime + velocity.vz; - - - sensor_reading.orientation_velocity = velocity; - - sensor_reading.gx = -event.un.gyroscope.y; - sensor_reading.gy = event.un.gyroscope.x; - sensor_reading.gz = event.un.gyroscope.z; - - sensor_reading.magnetometer.mx = -event.un.magneticField.y; - sensor_reading.magnetometer.my = event.un.magneticField.x; - sensor_reading.magnetometer.mz = event.un.magneticField.z; - - sensor_reading.temperature = event.un.temperature.value; - sensor_reading.pressure = event.un.pressure.value; - // sets the initial position of the system - if (initial_flag == 0) - { - initial_orientation = sensor_reading; - initial_flag = 1; - } - - Vec3 rotated_data {-euler.z, -euler.y, euler.x}; // roll, pitch, yaw + } - // The guess & check method! - // Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle! + sensor_reading.euler.yaw = -euler.y; + sensor_reading.euler.pitch = euler.x; + sensor_reading.euler.roll = euler.z; - Eigen::Matrix3f rot_matrix = generate_rotation_matrix(rotated_data); - Eigen::Matrix cur_ivec = {1, 0, 0}; - Eigen::Matrix cur_vec = cur_ivec * rot_matrix; - Eigen::Matrix reference_vector = {0, 0, -1}; + sensor_reading.linear_acceleration.ax = -event.un.accelerometer.y; + sensor_reading.linear_acceleration.ay = event.un.accelerometer.x; + sensor_reading.linear_acceleration.az = event.un.accelerometer.z; - float dot = cur_vec.dot(reference_vector); - float cur_mag = cur_vec.norm(); - float ref_mag = reference_vector.norm(); + Velocity velocity; + velocity.vx = sensor_reading.linear_acceleration.ax * deltaTime + velocity.vx; + velocity.vy = sensor_reading.linear_acceleration.ay * deltaTime + velocity.vy; + velocity.vz = sensor_reading.linear_acceleration.az * deltaTime + velocity.vz; - sensor_reading.tilt = 0; - if(cur_mag != 0 && ref_mag != 0) { - sensor_reading.tilt = acos(dot/(cur_mag*ref_mag)); - } + sensor_reading.orientation_velocity = velocity; + + sensor_reading.gx = -event.un.gyroscope.y; + sensor_reading.gy = event.un.gyroscope.x; + sensor_reading.gz = event.un.gyroscope.z; + + sensor_reading.magnetometer.mx = -event.un.magneticField.y; + sensor_reading.magnetometer.my = event.un.magneticField.x; + sensor_reading.magnetometer.mz = event.un.magneticField.z; + + sensor_reading.temperature = event.un.temperature.value; + sensor_reading.pressure = event.un.pressure.value; + + Vec3 rotated_data{-euler.z, -euler.y, euler.x}; // roll, pitch, yaw - const float alpha = 0.2; - // Arthur's Comp Filter - float filtered_tilt = alpha * sensor_reading.tilt + (1-alpha) * prev_tilt; - prev_tilt = filtered_tilt; + // The guess & check method! + // Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle! - // Serial.print("TILT: "); - // Serial.println(filtered_tilt * (180/3.14f)); - - return sensor_reading; + Eigen::Matrix3f rot_matrix = generate_rotation_matrix(rotated_data); + Eigen::Matrix cur_ivec = {1, 0, 0}; + Eigen::Matrix cur_vec = cur_ivec * rot_matrix; + Eigen::Matrix reference_vector = {0, 0, -1}; + + float dot = cur_vec.dot(reference_vector); + float cur_mag = cur_vec.norm(); + float ref_mag = reference_vector.norm(); + + sensor_reading.tilt = 0; + if (cur_mag != 0 && ref_mag != 0) { + sensor_reading.tilt = acos(dot / (cur_mag * ref_mag)); } - return {.has_data = false}; + + return sensor_reading; +} + +bool HwImpl::is_orientation_ready() { + return imu.getSensorEvent(&event); } diff --git a/MIDAS/src/hardware/SDLog.cpp b/MIDAS/src/hardware/SDLog.cpp index 323dea5e..2954d246 100644 --- a/MIDAS/src/hardware/SDLog.cpp +++ b/MIDAS/src/hardware/SDLog.cpp @@ -3,6 +3,7 @@ #include #include "SDLog.h" +#include "pins.h" /** * @brief Initializes the SD card logger diff --git a/MIDAS/src/hardware/Voltage.cpp b/MIDAS/src/hardware/Voltage.cpp deleted file mode 100644 index aa2dd4ca..00000000 --- a/MIDAS/src/hardware/Voltage.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include "sensors.h" -#include -#include -#include - -#define VOLTAGE_DIVIDER (5.0 / (5.0 + 20.0)) - -int read_board_pwr_monitor_register(int reg, int bytes) { - Wire1.beginTransmission(0x44); - Wire1.write(reg); - if(Wire1.endTransmission()){ - Serial.println("I2C Error"); - } - - Wire1.requestFrom(0x44, bytes); - int val = 0; - - for(int i = 0; i < bytes; i++){ - int v = Wire1.read(); - if(v == -1) Serial.println("I2C Read Error"); - val = (val << 8) | v; - } - - return val; -} - -/** - * @brief "Initializes" the voltage sensor. Since it reads directly from a pin without a library, there is no specific initialization. - * - * @return Error Code, will always be NoError -*/ -ErrorCode VoltageSensor::init() { - return ErrorCode::NoError; -} - -/** - * @brief Reads the value of the given analog pin and converts it to a battery voltage with the assumption that the voltage sensor is plugged into that pin - * - * @return The scaled voltage given by the voltage sensor -*/ -Voltage VoltageSensor::read() { - Voltage v_battery; - int voltage = read_board_pwr_monitor_register(0x5, 2); - int16_t current = read_board_pwr_monitor_register(0x7, 2); - - float voltage_normalized = voltage * 3.125 / 1000.0; - float absolute_current = current * 1.2 / 1000.0; - - // Serial.print("Voltage: "); - // Serial.println(voltage_normalized); - // Serial.print("Current: "); - // Serial.println(current); - - v_battery.voltage = voltage_normalized; - v_battery.current = absolute_current; -// Serial.print("Raw voltage reading: "); -// Serial.print(v_battery.voltage); -// Serial.println(""); - //* 3.3f / 4095.f / VOLTAGE_DIVIDER; - return v_battery; -} diff --git a/MIDAS/src/hardware/main.cpp b/MIDAS/src/hardware/main.cpp index 25b85d99..cb1c777e 100644 --- a/MIDAS/src/hardware/main.cpp +++ b/MIDAS/src/hardware/main.cpp @@ -1,31 +1,23 @@ #include #include -#include "TCAL9539.h" +#include #include "systems.h" #include "hardware/pins.h" -#include "hardware/Emmc.h" #include "hardware/SDLog.h" -#include "sensor_data.h" -#include "pins.h" /** * Sets the config file and then starts all the threads using the config. */ -// #ifdef IS_SUSTAINER -// MultipleLogSink sinks; -MultipleLogSink sinks; -// #else -// MultipleLogSink<> sinks; -// #endif -RocketSystems systems{.log_sink = sinks}; +HwImpl hw_impl; +SDSink sinks; +RocketSystems systems(hw_impl, sinks); + /** * @brief Sets up pinmodes for all sensors and starts threads */ - -void setup() -{ +void setup() { // begin serial port Serial.begin(9600); @@ -59,23 +51,23 @@ void setup() //set all chip selects high (deselected) pinMode(LSM6DS3_CS, OUTPUT); - pinMode(KX134_CS, OUTPUT); - pinMode(ADXL355_CS, OUTPUT); - pinMode(LIS3MDL_CS, OUTPUT); - pinMode(BNO086_CS, OUTPUT); - pinMode(BNO086_RESET, OUTPUT); - pinMode(CAN_CS, OUTPUT); - pinMode(E22_CS, OUTPUT); - pinMode(MS5611_CS, OUTPUT); - - digitalWrite(MS5611_CS, HIGH); - digitalWrite(LSM6DS3_CS, HIGH); - digitalWrite(KX134_CS, HIGH); - digitalWrite(ADXL355_CS, HIGH); - digitalWrite(LIS3MDL_CS, HIGH); - digitalWrite(BNO086_CS, HIGH); - digitalWrite(CAN_CS, HIGH); - digitalWrite(E22_CS, HIGH); + pinMode(KX134_CS, OUTPUT); + pinMode(ADXL355_CS, OUTPUT); + pinMode(LIS3MDL_CS, OUTPUT); + pinMode(BNO086_CS, OUTPUT); + pinMode(BNO086_RESET, OUTPUT); + pinMode(CAN_CS, OUTPUT); + pinMode(E22_CS, OUTPUT); + pinMode(MS5611_CS, OUTPUT); + + digitalWrite(MS5611_CS, HIGH); + digitalWrite(LSM6DS3_CS, HIGH); + digitalWrite(KX134_CS, HIGH); + digitalWrite(ADXL355_CS, HIGH); + digitalWrite(LIS3MDL_CS, HIGH); + digitalWrite(BNO086_CS, HIGH); + digitalWrite(CAN_CS, HIGH); + digitalWrite(E22_CS, HIGH); //configure output leds gpioPinMode(LED_BLUE, OUTPUT); gpioPinMode(LED_GREEN, OUTPUT); @@ -91,9 +83,8 @@ void setup() delay(200); // init and start threads - begin_systems(&systems); + systems.begin(); } -void loop() -{ +void loop() { } diff --git a/MIDAS/src/hardware/sensors.cpp b/MIDAS/src/hardware/sensors.cpp new file mode 100644 index 00000000..757d4b09 --- /dev/null +++ b/MIDAS/src/hardware/sensors.cpp @@ -0,0 +1,425 @@ +#include "sensors.h" +#include "pins.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include "E22.h" + +// Change to 434.0 or other frequency, must match RX's freq! +#ifdef IS_BOOSTER +#define TX_FREQ 425.15 +#else +#define TX_FREQ 421.15 +#endif + +// Which b2b communication we should use +#define B2B_I2C +// #define B2B_CAN + +#if defined(B2B_I2C) && defined(B2B_CAN) +#error "B2B can only use one option of B2B_I2C, or B2B_CAN" +#elif !defined(B2B_I2C) && !defined(B2B_CAN) +#error "At least one B2B_I2C or B2B_CAN must be defined" +#endif + +#if defined(B2B_CAN) +#error "you've been baited lmfao" +#endif + +PL::ADXL355 low_g(ADXL355_CS); +LSM6DS3Class low_g_lsm(SPI, LSM6DS3_CS, 46); +QwiicKX134 KX; +MS5611 MS(MS5611_CS); +Adafruit_LIS3MDL LIS3MDL; +SFE_UBLOX_GNSS ublox; +SX1268 lora(SPI, E22_CS, E22_BUSY, E22_DI01, E22_RXEN, E22_RESET); + +ErrorCode init_low_g() { + low_g.begin(); + low_g.setRange(PL::ADXL355_Range::range2g); + low_g.setOutputDataRate(PL::ADXL355_OutputDataRate::odr1000); + // todo set low pass filter frequency to 250hx + low_g.enableMeasurement(); + return ErrorCode::NoError; +} + +LowGData HwImpl::read_low_g() { + auto data = low_g.getAccelerations(); + return { data.x, data.y, data.z }; +} + +ErrorCode init_low_g_lsm() { + if (!low_g_lsm.begin()) { + return ErrorCode::GyroCouldNotBeInitialized; + } + return ErrorCode::NoError; +} + +LowGLSMData HwImpl::read_low_g_lsm() { + LowGLSMData result; + low_g_lsm.readAcceleration(result.ax, result.ay, result.az); + low_g_lsm.readGyroscope(result.gx, result.gy, result.gz); + return result; +} + +ErrorCode init_high_g() { + KX.beginSPI(KX134_CS); + if (!KX.initialize(DEFAULT_SETTINGS)) { + return ErrorCode::HighGCouldNotBeInitialized; + } + + if(!KX.setOutputDataRate(0xb)) { + return ErrorCode::HighGCouldNotUpdateDataRate; + } + + KX.setRange(3); + return ErrorCode::NoError; +} + +HighGData HwImpl::read_high_g() { + auto data = KX.getAccelData(); + return { data.xData, data.yData, data.zData }; +} + +ErrorCode init_barometer() { + MS.init(); + + return ErrorCode::NoError; +} + +BarometerData HwImpl::read_barometer() { + MS.read(12); + + /* + * TODO: Switch to latest version of library (0.3.9) when we get hardware to verify + * Equation derived from https://en.wikipedia.org/wiki/Atmospheric_pressure#Altitude_variation + */ + float pressure = static_cast(MS.getPressure() * 0.01 + 26.03); // getPressure is in milibars so it's milibars * 0.01? + float temperature = static_cast(MS.getTemperature() * 0.01); // Celcius + float altitude = static_cast(-log(pressure * 0.000987) * (temperature + 273.15) * 29.254); + + return { temperature, pressure, altitude }; +} + +ErrorCode init_continuity() { + // ADS7138Init(); // Ask ADS to init the pins, we still need to get the device to actually read + // Configure the INA745b MODE:ContTCV VBUS:1052us VSEN:1052us TCT:1052us AVG:128 + constexpr uint16_t INA_config = (0xF << 12) | (0x5 << 9) | (0x5 << 6) | (0x5 << 3) | (0x4); + + Wire1.beginTransmission(0x41); + Wire1.write(0x1); + + Wire1.write(((INA_config >> 8) & 0xFF)); + Wire1.write(((INA_config >> 0) & 0xFF)); + + if(Wire1.endTransmission()){ + Serial.println("Pyro PWR I2C Error"); + return ErrorCode::ContinuityCouldNotBeInitialized; + } + + Serial.println("Pyro PWR monitor configured"); + + return ErrorCode::NoError; +} + +int read_pwr_monitor_register(int address, int reg, int bytes) { + Wire1.beginTransmission(address); // I2C Address 0x41 is pyro pwr monitor + Wire1.write(reg); + if(Wire1.endTransmission()){ + Serial.println("I2C Error"); + } + + Wire1.requestFrom(address, bytes); + int val = 0; + + for(int i = 0; i < bytes; i++){ + int v = Wire1.read(); + if(v == -1) Serial.println("I2C Read Error"); + val = (val << 8) | v; + } + + return val; +} + +ContinuityData HwImpl::read_continuity() { + ContinuityData continuity; + //ADC reference voltage is 3.3, returns 12 bit value + + // MIDAS 2.1 rev A ADC sense fix: + int16_t current = read_pwr_monitor_register(0x41, 0x7, 2); + int voltage = read_pwr_monitor_register(0x41, 0x5, 2); + + float voltage_normalized = voltage * 3.125 / 1000.0; // V + + float continuous_channels = 0.0; + float absolute_current = 0.0; + float expected_current = 0.0; + + if(voltage_normalized > 1) { + absolute_current = current * 1.2 / 1000.0; + + expected_current = (voltage_normalized - 0.2) / 470.0; // Account for diode voltage drop + continuous_channels = absolute_current / expected_current; + } + + // We don't have the granularity to determine individual voltages, so all of them will give the # of continuous channels + continuity.pins[0] = continuous_channels; // Number of continuous channels on the pyro bus + continuity.pins[1] = absolute_current; // The absolute current running through the pyro bus + continuity.pins[2] = expected_current; // Calculated expected current based on current pyro bus voltage + continuity.pins[3] = voltage_normalized; // Pyro bus voltage + + // Serial.println(continuous_channels); + return continuity; +} + +VoltageData HwImpl::read_voltage() { + VoltageData v_battery; + int voltage = read_pwr_monitor_register(0x44, 0x5, 2); + int16_t current = read_pwr_monitor_register(0x44, 0x7, 2); + + float voltage_normalized = voltage * 3.125 / 1000.0; + float absolute_current = current * 1.2 / 1000.0; + + // Serial.print("Voltage: "); + // Serial.println(voltage_normalized); + // Serial.print("Current: "); + // Serial.println(current); + + v_battery.voltage = voltage_normalized; + v_battery.current = absolute_current; +// Serial.print("Raw voltage reading: "); +// Serial.print(v_battery.voltage); +// Serial.println(""); + //* 3.3f / 4095.f / VOLTAGE_DIVIDER; + return v_battery; +} + +ErrorCode init_magnetometer() { + if (!LIS3MDL.begin_SPI(LIS3MDL_CS)) { // Checks if sensor is connected + return ErrorCode::MagnetometerCouldNotBeInitialized; + } + LIS3MDL.setOperationMode(LIS3MDL_CONTINUOUSMODE); // Reading continuously, instead of single-shot or off + LIS3MDL.setDataRate(LIS3MDL_DATARATE_155_HZ); + LIS3MDL.setRange(LIS3MDL_RANGE_4_GAUSS); // Earth's magnetic field is 1/2 gauss, can detect high current + return ErrorCode::NoError; +} + +MagnetometerData HwImpl::read_magnetometer() { + LIS3MDL.read(); + + float mx = LIS3MDL.x_gauss; + float my = LIS3MDL.y_gauss; + float mz = LIS3MDL.z_gauss; + MagnetometerData reading { mx, my, mz }; + return reading; +} + +ErrorCode init_gps() { + if (!ublox.begin()) { + return ErrorCode::GPSCouldNotBeInitialized; + } + + ublox.setDynamicModel(DYN_MODEL_AIRBORNE4g); + ublox.setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA); //Set the I2C port to output both NMEA and UBX messages + // Set the measurment rate faster than one HZ if necessary + // ublox.setMeasurementRate(100); + ublox.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR + + //This will pipe all NMEA sentences to the serial port so we can see them + + return ErrorCode::NoError; +} + +bool HwImpl::is_gps_ready() { + return ublox.getPVT(); +} + +GPSData HwImpl::read_gps() { + return { + ublox.getLatitude(), + ublox.getLongitude(), + (float) ublox.getAltitude() / 1000.f, + (float) ublox.getGroundSpeed() / 1000.f, + ublox.getFixType(), + ublox.getUnixEpoch() + }; +} + +static GpioAddress LED_pins[4] = { + LED_BLUE, + LED_RED, + LED_ORANGE, + LED_GREEN +}; + +void HwImpl::set_led(LED which, bool value) { + gpioDigitalWrite(LED_pins[(int) which], value); +} + +ErrorCode init_lora() { + if (lora.setup() != SX1268Error::NoError) { + return ErrorCode::LoraCouldNotBeInitialized; + } + if (lora.set_modulation_params(8, LORA_BW_250, LORA_CR_4_8, false) != SX1268Error::NoError) { + return ErrorCode::LoraCommunicationFailed; + } + if (lora.set_frequency((uint32_t) (TX_FREQ * 1e6)) != SX1268Error::NoError) { + return ErrorCode::LoraCommunicationFailed; + } + if (lora.set_tx_power(22) != SX1268Error::NoError) { + return ErrorCode::LoraCommunicationFailed; + } + + return ErrorCode::NoError; +} + +void HwImpl::transmit_bytes(uint8_t* data, size_t count) { + SX1268Error result = lora.send(data, count); + if (result != SX1268Error::NoError) { + Serial.print("Lora TX error "); + Serial.println((int) result); + // Reinit the lora + init_lora(); + } +} + +bool HwImpl::receive_bytes(uint8_t* memory, size_t count, int wait_milliseconds) { + SX1268Error result = lora.recv((uint8_t*) write, count, wait_milliseconds); + if (result == SX1268Error::NoError) { + return true; + } else if (result == SX1268Error::RxTimeout) { + return false; + } else { + Serial.print("Lora error on rx "); + Serial.println((int) result); + + // Reinit the lora + init_lora(); + return false; + } +} + +bool error_is_failure(GpioError error_code) { + return error_code != GpioError::NoError; +} + +ErrorCode init_pyro() { + bool has_failed_gpio_init = false; + + // global arm + has_failed_gpio_init |= error_is_failure(gpioPinMode(PYRO_GLOBAL_ARM_PIN, OUTPUT)); + + // fire pins + has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROA_FIRE_PIN, OUTPUT)); + has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROB_FIRE_PIN, OUTPUT)); + has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROC_FIRE_PIN, OUTPUT)); + has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROD_FIRE_PIN, OUTPUT)); + +// if (has_failed_gpio_init) { +// return ErrorCode::PyroGPIOCouldNotBeInitialized; +// } else { + return ErrorCode::NoError; // GPIO Driver always claimes it errored even when it doesn't. +// } +} + +void HwImpl::set_global_arm(bool to_high) { + gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, to_high); +} + +void HwImpl::set_pin_firing(Channel which, bool to_high) { + GpioAddress address = PYROA_FIRE_PIN; + switch (which) { + case Channel::A: + address = PYROA_FIRE_PIN; + break; + case Channel::B: + address = PYROB_FIRE_PIN; + break; + case Channel::C: + address = PYROC_FIRE_PIN; + break; + case Channel::D: + address = PYROD_FIRE_PIN; + break; + } + gpioDigitalWrite(address, to_high); +} + +enum CameraCommand { + CAMERA1_OFF = 0, + CAMERA1_ON = 1, + CAMERA2_OFF = 2, + CAMERA2_ON = 3, + VTX_OFF = 4, + VTX_ON = 5, + MUX_1 = 6, + MUX_2 = 7 +}; + +void transmit_command(CameraCommand command) { +#ifdef B2B_I2C + Wire.beginTransmission(0x69); // 0x69 --> Camera board i2c address + Wire.write((uint8_t) command); + if (Wire.endTransmission()) { + Serial.println("Camera B2B i2c write error"); + } +#else + #error "lmao" +#endif +} + +void HwImpl::set_camera_on(Camera which, bool on) { + CameraCommand command; + if (which == Camera::Side) { + command = on ? CameraCommand::CAMERA1_ON : CameraCommand::CAMERA1_OFF; + } else { + command = on ? CameraCommand::CAMERA2_ON : CameraCommand::CAMERA2_OFF; + } + transmit_command(command); +} + +void HwImpl::set_camera_source(Camera which) { + // it's apparently backwards for some reason? + CameraCommand command = which == Camera::Side ? CameraCommand::MUX_2 : CameraCommand::MUX_1; + transmit_command(command); +} + +void HwImpl::set_video_transmit(bool on) { + CameraCommand command = on ? CameraCommand::VTX_ON : CameraCommand::VTX_OFF; + transmit_command(command); +} + +uint8_t HwImpl::get_camera_state() { +#ifdef B2B_I2C + Wire.requestFrom(0x69, 1); + uint8_t res = Wire.read(); + return res; +#else + #error "lmao" +#endif +} + +#define TRY(fn) do { ErrorCode code_ = fn(); if (code_ != ErrorCode::NoError) { return code_; } } while (0) + +ErrorCode init_orientation(); + +ErrorCode HwImpl::init_all() { + TRY(init_low_g); + TRY(init_low_g_lsm); + TRY(init_high_g); + TRY(init_barometer); + TRY(init_continuity); +// TRY(init_voltage); + TRY(init_orientation); + TRY(init_magnetometer); + TRY(init_gps); + TRY(init_pyro); + TRY(init_lora); + return ErrorCode::NoError; +} diff --git a/MIDAS/src/hardware/sensors.h b/MIDAS/src/hardware/sensors.h index 629b5df2..f1b5ee1b 100644 --- a/MIDAS/src/hardware/sensors.h +++ b/MIDAS/src/hardware/sensors.h @@ -1,108 +1,34 @@ #pragma once -#include "errors.h" -#include "sensor_data.h" -#include "hardware/pins.h" -#include "TCAL9539.h" -#include "rocket_state.h" +#include "hal.h" -/** - * @struct LowG interface - */ -struct LowGSensor { - ErrorCode init(); - LowGData read(); -}; - -/** - * @struct HighG interface - */ -struct HighGSensor { - ErrorCode init(); - HighGData read(); -}; - -/** - * @struct Magnetometer interface - */ -struct MagnetometerSensor { - ErrorCode init(); - Magnetometer read(); -}; - -/** - * @struct Barometer interface - */ -struct BarometerSensor { - ErrorCode init(); - Barometer read(); -}; - -/** - * @struct LowGLSM interface - */ -struct LowGLSMSensor { - ErrorCode init(); - LowGLSM read(); -}; - -/** - * @struct Continuity interface - */ -struct ContinuitySensor { - ErrorCode init(); - Continuity read(); -}; - -/** - * @struct Voltage interface - */ -struct VoltageSensor { - ErrorCode init(); - Voltage read(); -}; - -/** - * @struct BNO interface - */ -struct OrientationSensor { - Orientation initial_orientation; - Quaternion initial_quaternion; - uint8_t initial_flag; +struct HwImpl : HwInterface { + HwImpl() = default; - float prev_x = 0; - float prev_y = 0; - float prev_z = 0; - float prev_tilt = 0; + ErrorCode init_all(); - ErrorCode init(); - Orientation read(); -}; + LowGData read_low_g(); + LowGLSMData read_low_g_lsm(); + HighGData read_high_g(); + BarometerData read_barometer(); + ContinuityData read_continuity(); + VoltageData read_voltage(); + bool is_orientation_ready(); + OrientationData read_orientation(); + MagnetometerData read_magnetometer(); + bool is_gps_ready(); + GPSData read_gps(); -/** - * @struct GPS interface - */ -struct GPSSensor { - ErrorCode init(); - bool valid(); - GPS read(); - bool is_leap = false; -}; + void set_led(LED which, bool value); -/** - * @struct Pyro interface - */ -struct Pyro { - ErrorCode init(); - PyroState tick(FSMState fsm_state, Orientation orientation, CommandFlags& telem_commands); + void transmit_bytes(uint8_t* memory, size_t count); + bool receive_bytes(uint8_t* memory, size_t count, int wait_milliseconds); - void set_pyro_safety(); // Sets pyro_start_firing_time and has_fired_pyros. - void reset_pyro_safety(); // Resets pyro_start_firing_time and has_fired_pyros. - - private: - void disarm_all_channels(PyroState& prev_state); - void fire_pyro(int channel_idx, GpioAddress arm_pin, GpioAddress fire_pin); + void set_global_arm(bool to_high); + void set_pin_firing(Channel which, bool to_high); - double safety_pyro_start_firing_time; // Time when pyros have fired "this cycle" (pyro test) -- Used to only fire pyros for a time then transition to SAFE - bool safety_has_fired_pyros_this_cycle; // If pyros have fired "this cycle" (pyro test) -- Allows only firing 1 pyro per cycle. + void set_camera_on(Camera which, bool on); + void set_camera_source(Camera which); + void set_video_transmit(bool on); + uint8_t get_camera_state(); }; diff --git a/MIDAS/src/hardware/telemetry_backend.cpp b/MIDAS/src/hardware/telemetry_backend.cpp deleted file mode 100644 index 4a3cd6fe..00000000 --- a/MIDAS/src/hardware/telemetry_backend.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/** - * @file telemetry.cpp - * - * @brief This file defines the telemetry class used to facilitate - * telemetry commands and data transfer between the on-board flight - * computer and the ground station. - * - * Spaceshot Avionics 2023-24 - * Illinois Space Society - Software Team - * Gautam Dayal - * Nicholas Phillips - * Patrick Marschoun - * Peter Giannetos - * Rishi Gokkumutkkala - * Aaditya Voruganti - * Magilan Sendhil - */ - -#include "hardware/telemetry_backend.h" -#include "hardware/pins.h" - -// Change to 434.0 or other frequency, must match RX's freq! -#ifdef IS_BOOSTER -#define TX_FREQ 425.15 -#else -#define TX_FREQ 421.15 -#endif - -#define TX_OUTPUT_POWER 22 // dBm -#define LORA_BANDWIDTH 0 // [0: 125 kHz, 1: 250 kHz, 2: 500 kHz, 3: Reserved] -#define LORA_SPREADING_FACTOR 8// [SF7..SF12] -#define LORA_CODINGRATE 4 // [1: 4/5, 2: 4/6, 3: 4/7, 4: 4/8] -#define LORA_PREAMBLE_LENGTH 10 // Same for Tx and Rx -#define LORA_SYMBOL_TIMEOUT 0 // Symbols -#define LORA_FIX_LENGTH_PAYLOAD_ON false -#define LORA_IQ_INVERSION_ON false -#define RX_TIMEOUT_VALUE 1000 -#define TX_TIMEOUT_VALUE 1000 -#define LORA_BUFFER_SIZE 64 // Define the payload size here - -/** - * @brief Default constructor for the telemetry system -*/ -TelemetryBackend::TelemetryBackend() : lora(SPI, E22_CS, E22_BUSY, E22_DI01, E22_RXEN, E22_RESET) { - led_state = false; -} -/** - * @brief Initializes the telemetry system - * - * @return Error Code -*/ -ErrorCode TelemetryBackend::init() { - if(lora.setup() != SX1268Error::NoError) return ErrorCode::LoraCouldNotBeInitialized; - if(lora.set_modulation_params(8, LORA_BW_250, LORA_CR_4_8, false) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; - if(lora.set_frequency((uint32_t) (TX_FREQ * 1e6)) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; - if(lora.set_tx_power(22) != SX1268Error::NoError) return ErrorCode::LoraCommunicationFailed; - - return ErrorCode::NoError; -} - -/** - * @brief Gets RSSI of recent packets - * - * @return RSSI of most recent packet -*/ -int16_t TelemetryBackend::getRecentRssi() { - return 0; -} - -/** - * @brief Sets new frequency for the LoRa module - * - * @param freq New frequency to set the LoRa module to -*/ -ErrorCode TelemetryBackend::setFrequency(float freq) { - if(lora.set_frequency((uint32_t) (freq * 1e6)) != SX1268Error::NoError) { - return ErrorCode::LoraCommunicationFailed; - } else { - return ErrorCode::NoError; - } -} diff --git a/MIDAS/src/hardware/telemetry_backend.h b/MIDAS/src/hardware/telemetry_backend.h deleted file mode 100644 index 84ad55c6..00000000 --- a/MIDAS/src/hardware/telemetry_backend.h +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include "errors.h" -#include "hal.h" -#include "pins.h" -#include - -#include "E22.h" - -/** - * @class TelemetryBackend - * - * @brief Class that wraps the Telemetry functions -*/ -class TelemetryBackend { -public: - TelemetryBackend(); - [[nodiscard]] ErrorCode init(); - - int16_t getRecentRssi(); - ErrorCode setFrequency(float frequency); - - /** - * @brief This function transmits data from the struct provided as - * the parameter (data collected from sensor suite) to the - * ground station. The function also switches to a new commanded - * frequency based on a previously received command and waits for - * a response from the ground station. - * - * @param sensor_data: struct of data from the sensor suite to be - * transmitted to the ground station. - * - * @return void - */ - template - void send(const T& data) { - static_assert(sizeof(T) <= 0xFF, "The data type to send is too large"); // Max payload is 255 - gpioDigitalWrite(LED_BLUE, led_state); - led_state = !led_state; - - SX1268Error result = lora.send((uint8_t*) &data, sizeof(T)); - if(result != SX1268Error::NoError) { - Serial.print("Lora TX error "); - Serial.println((int)result); - // Re init the lora - (void)init(); - } - } - - /** - * @brief Reads message from the LoRa - * - * @param write The buffer to write the data to - * - * @return bool indicating a successful read and write to buffer - */ - template - bool read(T* write, int wait_milliseconds) { - static_assert(sizeof(T) <= 0xFF, "The data type to receive is too large"); - uint8_t len = sizeof(T); - // set receive mode - SX1268Error result = lora.recv((uint8_t*) write, len, wait_milliseconds); - if(result == SX1268Error::NoError) { - return true; - } else if(result == SX1268Error::RxTimeout) { - return false; - } else { - Serial.print("Lora error on rx "); - Serial.println((int)result); - - //Re init the lora - (void)init(); - return false; - } - } - -private: - SX1268 lora; - bool led_state; -}; diff --git a/MIDAS/src/led.cpp b/MIDAS/src/led.cpp deleted file mode 100644 index 7f3e2301..00000000 --- a/MIDAS/src/led.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include "led.h" -#include "TCAL9539.h" -#include "hardware/pins.h" - -/** - * @struct GpioAddress - * - * @brief struct representing the LED pins -*/ -static GpioAddress LED_pins[4] = { - LED_BLUE, - LED_RED, - LED_ORANGE, - LED_GREEN -}; - -/** - * @brief Initializes LEDs - * - * @return Error code -*/ -ErrorCode LEDController::init() { - return ErrorCode::NoError; -} - -/** - * @brief Toggles a specific LED's state -*/ -void LEDController::toggle(LED led) { - int id = static_cast(led); - if (targets[id] == LOW) { - targets[id] = HIGH; - } else { - targets[id] = LOW; - } -} - -/** - * @brief Sets a specific LED's state -*/ -void LEDController::set(LED led, int state) { - int id = static_cast(led); - targets[id] = state; -} - - -/** - * @brief updates the LEDS to represent the current state the rocket is in -*/ -void LEDController::update() { - for (int i = 0; i < 4; i++) { - if (targets[i] != states[i]) { - gpioDigitalWrite(LED_pins[i], targets[i]); - states[i] = targets[i]; - } - } -} diff --git a/MIDAS/src/led.h b/MIDAS/src/led.h index 42b34245..4e0db5a3 100644 --- a/MIDAS/src/led.h +++ b/MIDAS/src/led.h @@ -1,33 +1,37 @@ #pragma once #include "hal.h" -#include "errors.h" - -/** - * @enum LED - * - * @brief represents the different LEDS -*/ -enum class LED { - BLUE = 0, - RED = 1, - ORANGE = 2, - GREEN = 3 -}; /** * @class LEDController * * @brief wraps functionality for LEDs -*/ + */ +template class LEDController { - int states[4]; - int targets[4]; + bool states[4] = {}; + bool targets[4] = {}; + + HwInterface& hw; public: - ErrorCode init(); - void update(); + LEDController(HwInterface& hw) : hw(hw) { } + + void update() { + for (int i = 0; i < 4; i++) { + if (targets[i] != states[i]) { + hw.set_led((LED) i, targets[i]); + states[i] = targets[i]; + } + } + } + + void toggle(LED led) { + int id = static_cast(led); + targets[id] = !targets[id]; + } - void toggle(LED led); - void set(LED led, int state); + void set(LED led, bool state) { + targets[(int) led] = state; + } }; diff --git a/MIDAS/src/log_format.h b/MIDAS/src/log_format.h index dde361bb..6910ed5e 100644 --- a/MIDAS/src/log_format.h +++ b/MIDAS/src/log_format.h @@ -41,13 +41,13 @@ struct LoggedReading { union { LowGData low_g; HighGData high_g; - Barometer barometer; - Continuity continuity; - Voltage voltage; - GPS gps; - Magnetometer magnetometer; - Orientation orientation; - LowGLSM lowg_lsm; + BarometerData barometer; + ContinuityData continuity; + VoltageData voltage; + GPSData gps; + MagnetometerData magnetometer; + OrientationData orientation; + LowGLSMData lowg_lsm; KalmanData kalman; FSMState fsm; PyroState pyro; diff --git a/MIDAS/src/hardware/Pyro.cpp b/MIDAS/src/pyro.h similarity index 58% rename from MIDAS/src/hardware/Pyro.cpp rename to MIDAS/src/pyro.h index ef95e37f..b4c9a039 100644 --- a/MIDAS/src/hardware/Pyro.cpp +++ b/MIDAS/src/pyro.h @@ -1,90 +1,64 @@ +#pragma once + #include -#include "sensors.h" -#include "pins.h" +#include "sensor_data.h" +#include "rocket_state.h" +#include "hal.h" -#include "TCAL9539.h" -#include +template +struct PyroLogic { +private: + HwInterface& hw; + double safety_pyro_start_firing_time; // Time when pyros have fired "this cycle" (pyro test) -- Used to only fire pyros for a time then transition to SAFE + bool safety_has_fired_pyros_this_cycle; // If pyros have fired "this cycle" (pyro test) -- Allows only firing 1 pyro per cycle. -// Fire the pyros for this time during PYRO_TEST (ms) -#define PYRO_TEST_FIRE_TIME 100 +public: + explicit PyroLogic(HwInterface& hw) : hw(hw) { } + PyroState tick(FSMState fsm_state, OrientationData orientation, CommandFlags& telem_commands); -#define MAXIMUM_TILT_ANGLE (M_PI/5) // 36 degrees + void disarm_all_channels(PyroState& prev_state) { + hw.set_global_arm(false); + hw.set_pin_firing(Channel::A, false); + hw.set_pin_firing(Channel::B, false); + hw.set_pin_firing(Channel::C, false); + hw.set_pin_firing(Channel::D, false); -/** - * @brief Returns true if the error_code signals failure. - */ -bool error_is_failure(GpioError error_code) { - return error_code != GpioError::NoError; -} - -/** - * @brief Determines if orientation is in an acceptable range to ignite second stage. - * - * @return True if acceptable, false if not. - */ -bool can_fire_igniter(Orientation orientation) { - // With new GNC orientation code we can add a simple check. - return orientation.tilt < MAXIMUM_TILT_ANGLE; -} + prev_state.is_global_armed = false; + for (size_t i = 0; i < 4; ++i) { + // Update each channel's state sequentially + prev_state.channel_firing[i] = false; + } + } -/** - * @brief Initializes the pyro thread. The main initialization will be done by the GPIO expander, so the pyro thread doesn't - * have to do anything special and will always return NoError. - */ -ErrorCode Pyro::init() { - bool has_failed_gpio_init = false; - - // global arm - has_failed_gpio_init |= error_is_failure(gpioPinMode(PYRO_GLOBAL_ARM_PIN, OUTPUT)); - - // fire pins - has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROA_FIRE_PIN, OUTPUT)); - has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROB_FIRE_PIN, OUTPUT)); - has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROC_FIRE_PIN, OUTPUT)); - has_failed_gpio_init |= error_is_failure(gpioPinMode(PYROD_FIRE_PIN, OUTPUT)); - -// if (has_failed_gpio_init) { -// return ErrorCode::PyroGPIOCouldNotBeInitialized; -// } else { - return ErrorCode::NoError; // GPIO Driver always claimes it errored even when it doesn't. -// } -} + void set_pyro_safety() { + safety_pyro_start_firing_time = pdTICKS_TO_MS(xTaskGetTickCount()); + safety_has_fired_pyros_this_cycle = true; + } -void Pyro::disarm_all_channels(PyroState& prev_state) { - gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, LOW); - gpioDigitalWrite(PYROA_FIRE_PIN, LOW); - gpioDigitalWrite(PYROB_FIRE_PIN, LOW); - gpioDigitalWrite(PYROC_FIRE_PIN, LOW); - gpioDigitalWrite(PYROD_FIRE_PIN, LOW); - - prev_state.is_global_armed = false; - - for(size_t i = 0; i < 4; ++i) { - // Update each channel's state sequentially - prev_state.channel_firing[i] = false; + void reset_pyro_safety() { + safety_has_fired_pyros_this_cycle = false; } -} +}; -void Pyro::set_pyro_safety() { - safety_pyro_start_firing_time = pdTICKS_TO_MS(xTaskGetTickCount()); - safety_has_fired_pyros_this_cycle = true; +inline bool can_fire_igniter(OrientationData orientation) { + // With new GNC orientation code we can add a simple check. + return orientation.tilt < (M_PI/5); // 36 degrees; } -void Pyro::reset_pyro_safety() { - safety_has_fired_pyros_this_cycle = false; -} +#define PYRO_TEST_FIRE_TIME 100 #ifdef IS_SUSTAINER /** * @brief Upper stage only! Fires channels by setting their pin on the GPIO. - * + * * @return A pyro struct indicating which pyro channels are armed and/or firing. */ -PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& telem_commands) { +template +PyroState PyroLogic::tick(FSMState fsm_state, OrientationData orientation, CommandFlags& telem_commands) { PyroState new_pyro_state = PyroState(); double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); @@ -95,18 +69,18 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& // If the state is not SAFE, we arm the global arm pin new_pyro_state.is_global_armed = true; - gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, HIGH); + hw.set_global_arm(true); switch (fsm_state) { - case FSMState::STATE_IDLE: + case FSMState::STATE_IDLE: { reset_pyro_safety(); // Ensure that pyros can be fired when we transition away from this state break; - case FSMState::STATE_PYRO_TEST: - - if(safety_has_fired_pyros_this_cycle) { - // If a fire pyro command has already be acknowledged, do not acknowledge more commands, just fire pyro for the defined time + } + case FSMState::STATE_PYRO_TEST: { + if (safety_has_fired_pyros_this_cycle) { + // If a fire pyro command has already been acknowledged, do not acknowledge more commands, just fire pyro for the defined time // then, transition to SAFE. - if((current_time - safety_pyro_start_firing_time) >= PYRO_TEST_FIRE_TIME) { + if ((current_time - safety_pyro_start_firing_time) >= PYRO_TEST_FIRE_TIME) { telem_commands.should_transition_safe = true; disarm_all_channels(new_pyro_state); telem_commands.should_fire_pyro_a = false; @@ -120,56 +94,57 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& } // Respond to telem commands to fire igniters - if(telem_commands.should_fire_pyro_a) { + if (telem_commands.should_fire_pyro_a) { // Fire pyro channel "A" new_pyro_state.channel_firing[0] = true; - gpioDigitalWrite(PYROA_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::A, true); set_pyro_safety(); } - if(telem_commands.should_fire_pyro_b) { + if (telem_commands.should_fire_pyro_b) { // Fire pyro channel "B" new_pyro_state.channel_firing[1] = true; - gpioDigitalWrite(PYROB_FIRE_PIN, HIGH); - + hw.set_pin_firing(Channel::B, true); set_pyro_safety(); } - if(telem_commands.should_fire_pyro_c) { + if (telem_commands.should_fire_pyro_c) { // Fire pyro channel "C" new_pyro_state.channel_firing[2] = true; - gpioDigitalWrite(PYROC_FIRE_PIN, HIGH); - + hw.set_pin_firing(Channel::C, true); set_pyro_safety(); } - if(telem_commands.should_fire_pyro_d) { + if (telem_commands.should_fire_pyro_d) { // Fire pyro channel "D" new_pyro_state.channel_firing[0] = true; - gpioDigitalWrite(PYROD_FIRE_PIN, HIGH); - + hw.set_pin_firing(Channel::D, true); set_pyro_safety(); } - + break; - case FSMState::STATE_SUSTAINER_IGNITION: + } + case FSMState::STATE_SUSTAINER_IGNITION: { // Fire "Pyro C" to ignite sustainer (Pyro C is motor channel) // Additionally, check if orientation allows for firing if (can_fire_igniter(orientation)) { new_pyro_state.channel_firing[2] = true; - gpioDigitalWrite(PYROC_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::C, true); } break; - case FSMState::STATE_DROGUE_DEPLOY: + } + case FSMState::STATE_DROGUE_DEPLOY: { // Fire "Pyro A" to deploy upper stage drogue new_pyro_state.channel_firing[0] = true; - gpioDigitalWrite(PYROA_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::A, true); break; - case FSMState::STATE_MAIN_DEPLOY: + } + case FSMState::STATE_MAIN_DEPLOY: { // Fire "Pyro B" to deploy main. new_pyro_state.channel_firing[1] = true; - gpioDigitalWrite(PYROB_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::B, true); break; + } default: break; } @@ -183,12 +158,13 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& /** * Lower stage only! - * + * * @brief Fires channels by setting their pin on the GPIO. - * + * * @return A new pyro struct, with data depending on whether or not each pyro channel should be firing. */ -PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& telem_commands) { +template +PyroState PyroLogic::tick(FSMState fsm_state, OrientationData orientation, CommandFlags& telem_commands) { PyroState new_pyro_state = PyroState(); double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); @@ -199,7 +175,7 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& // If the state is not SAFE, we arm the global arm pin new_pyro_state.is_global_armed = true; - gpioDigitalWrite(PYRO_GLOBAL_ARM_PIN, HIGH); + hw.set_global_arm(true); // If the state is IDLE or any state after that, we arm the global arm pin switch (fsm_state) { case FSMState::STATE_IDLE: @@ -227,14 +203,14 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& if(telem_commands.should_fire_pyro_a) { // Fire pyro channel "A" new_pyro_state.channel_firing[0] = true; - gpioDigitalWrite(PYROA_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::A, true); set_pyro_safety(); } if(telem_commands.should_fire_pyro_b) { // Fire pyro channel "B" new_pyro_state.channel_firing[1] = true; - gpioDigitalWrite(PYROB_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::B, true); set_pyro_safety(); } @@ -242,7 +218,7 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& if(telem_commands.should_fire_pyro_c) { // Fire pyro channel "C" new_pyro_state.channel_firing[2] = true; - gpioDigitalWrite(PYROC_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::C, true); set_pyro_safety(); } @@ -250,26 +226,26 @@ PyroState Pyro::tick(FSMState fsm_state, Orientation orientation, CommandFlags& if(telem_commands.should_fire_pyro_d) { // Fire pyro channel "D" new_pyro_state.channel_firing[3] = true; - gpioDigitalWrite(PYROD_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::D, true); set_pyro_safety(); } - + break; case FSMState::STATE_FIRST_SEPARATION: // Fire "Pyro D" when separating stage 1 new_pyro_state.channel_firing[3] = true; - gpioDigitalWrite(PYROD_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::D, true); break; case FSMState::STATE_DROGUE_DEPLOY: // Fire "Pyro A" to deploy drogue new_pyro_state.channel_firing[0] = true; - gpioDigitalWrite(PYROA_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::A, true); break; case FSMState::STATE_MAIN_DEPLOY: // Fire "Pyro B" to deploy Main new_pyro_state.channel_firing[1] = true; - gpioDigitalWrite(PYROB_FIRE_PIN, HIGH); + hw.set_pin_firing(Channel::B, true); break; default: break; diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index 19ecef13..c3857570 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -1,10 +1,10 @@ #pragma once -#include - #include "sensor_data.h" #include "hal.h" #include "Buffer.h" +#include "Mutex.h" +#include "Queue.h" /** * @brief The RocketState struct stores everything that is needed by more than one system/thread of the Rocket. @@ -172,6 +172,7 @@ struct CommandFlags { bool FSM_should_set_cam_feed_cam1 = false; // Triggered at launch (IDLE --> FIRST_BOOST) bool FSM_should_swap_camera_feed = false; // Triggered COAST --> APOGEE }; + /** * @struct RocketData * @@ -186,15 +187,15 @@ struct RocketData { SensorData kalman; SensorData low_g; BufferedSensorData high_g; - BufferedSensorData barometer; - SensorData low_g_lsm; - SensorData continuity; + BufferedSensorData barometer; + SensorData low_g_lsm; + SensorData continuity; SensorData pyro; SensorData fsm_state; - SensorData gps; - SensorData magnetometer; - SensorData orientation; - SensorData voltage; + SensorData gps; + SensorData magnetometer; + SensorData orientation; + SensorData voltage; CommandFlags command_flags; uint8_t camera_state = 127; diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index 5cb33e19..9d2680db 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/sensor_data.h @@ -1,13 +1,11 @@ #pragma once +#define _USE_MATH_DEFINES #include #include -#include #include "finite-state-machines/fsm_states.h" -//#define CONTINUITY_PIN_COUNT 5 - /** * @brief * This header provides all the implementation for the data that comes from all of the sensors/ @@ -19,36 +17,27 @@ * @brief First 4 structs are base vector, pos, vel, and accel data to be used elsewhere */ struct Vec3 { - float x = 0; - float y = 0; - float z = 0; + float x; + float y; + float z; }; struct Position { - float px = 0; - float py = 0; - float pz = 0; + float px; + float py; + float pz; }; struct Velocity { - float vx = 0; - float vy = 0; - float vz = 0; - - float get_speed() { - return sqrt(vx * vx + vy * vy + vz * vz); - } + float vx; + float vy; + float vz; }; struct Acceleration { - float ax = 0; - float ay = 0; - float az = 0; - - // Get G-Force applied on the rocket - float get_magnitude() { - return sqrt(ax * ax + ay * ay + az * az); - } + float ax; + float ay; + float az; }; /** @@ -72,12 +61,9 @@ struct euler_t { * @brief data from the LowG sensor */ struct LowGData { - float ax = 0; - float ay = 0; - float az = 0; - - LowGData() = default; - LowGData(float x, float y, float z) : ax(x), ay(y), az(z) {}; + float ax; + float ay; + float az; }; /** @@ -86,12 +72,9 @@ struct LowGData { * @brief data from the HighG sensor */ struct HighGData { - float ax = 0; - float ay = 0; - float az = 0; - - HighGData() = default; - HighGData(float x, float y, float z) : ax(x), ay(y), az(z) {} + float ax; + float ay; + float az; }; /** @@ -99,13 +82,13 @@ struct HighGData { * * @brief data from the Low G LSM sensor */ -struct LowGLSM { - float gx = 0; - float gy = 0; - float gz = 0; - float ax = 0; - float ay = 0; - float az = 0; +struct LowGLSMData { + float gx; + float gy; + float gz; + float ax; + float ay; + float az; }; /** @@ -113,13 +96,10 @@ struct LowGLSM { * * @brief data from the barometer */ -struct Barometer { - float temperature = 0; // Temperature in Celcius - float pressure = 0; // Pressure in millibars - float altitude = 0; // Altitude in meters (above sea level?) - - Barometer() = default; - Barometer(float t, float p, float a) : temperature(t), pressure(p), altitude(a) {} +struct BarometerData { + float temperature; // Temperature in Celcius + float pressure; // Pressure in millibars + float altitude; // Altitude in meters (above sea level?) }; /** @@ -127,31 +107,21 @@ struct Barometer { * * @brief data about pyro continuity */ -struct Continuity { +struct ContinuityData { float pins[4]; }; -/** - * @struct Voltage - * - * @brief data about battery voltage -*/ -struct Voltage { +struct VoltageData { float voltage = 0; float current = 0; }; -/** - * @struct GPS - * - * @brief data from the GPS -*/ -struct GPS { - int32_t latitude = 0; - int32_t longitude = 0; - float altitude = 0; // Altitude in meters - float speed = 0; // Speed in meters/second - uint16_t fix_type = 0; +struct GPSData { + int32_t latitude; + int32_t longitude; + float altitude; // Altitude in meters + float speed; // Speed in meters/second + uint16_t fix_type; // Unix timestamp since 1970 // This isn't included in the telem packet because this is // solely for the SD logger. We do not need to know what time it is @@ -159,12 +129,7 @@ struct GPS { uint32_t time; }; -/** - * @struct Magnetometer - * - * @brief data from the magnetometer -*/ -struct Magnetometer { +struct MagnetometerData { float mx; float my; float mz; @@ -176,9 +141,6 @@ struct Quaternion { static float dot(const Quaternion& q1, const Quaternion& q2) { return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; } - - - }; /** @@ -194,43 +156,26 @@ enum class OrientationReadingType { * * @brief data from the BNO */ -struct Orientation { - bool has_data = false; - OrientationReadingType reading_type = OrientationReadingType::FULL_READING; - - float yaw = 0; - float pitch = 0; - float roll = 0; - //For yessir.cpp - euler_t getEuler() const { - euler_t euler; - euler.yaw = this->yaw; - euler.pitch = this->pitch; - euler.roll = this->roll; - return euler; - } +struct OrientationData { + bool has_data; + OrientationReadingType reading_type /* = OrientationReadingType::FULL_READING */; + euler_t euler; Velocity orientation_velocity; Velocity angular_velocity; - Velocity getVelocity() const { - return orientation_velocity; - } - Acceleration orientation_acceleration; - Acceleration linear_acceleration; - float gx = 0, gy = 0, gz = 0; - - Magnetometer magnetometer; + float gx, gy, gz; - float temperature = 0; - float pressure = 0; + MagnetometerData magnetometer; - float tilt = 0; + float temperature; + float pressure; + float tilt; }; /** @@ -252,7 +197,7 @@ struct KalmanData { * @brief data regarding all pyro channels */ struct PyroState { - bool is_global_armed = false; + bool is_global_armed; bool channel_firing[4]; /** * By convention, the pyro states are as follows: diff --git a/MIDAS/src/silsim/arduino_emulation.cpp b/MIDAS/src/silsim/arduino_emulation.cpp index 6f7d92ed..7249943a 100644 --- a/MIDAS/src/silsim/arduino_emulation.cpp +++ b/MIDAS/src/silsim/arduino_emulation.cpp @@ -9,5 +9,6 @@ uint32_t millis(){ } void SerialPatch::begin(int baudrate) {} +void SerialPatch::flush() {} SerialPatch Serial; diff --git a/MIDAS/src/silsim/arduino_emulation.h b/MIDAS/src/silsim/arduino_emulation.h index a118008e..aa3afd87 100644 --- a/MIDAS/src/silsim/arduino_emulation.h +++ b/MIDAS/src/silsim/arduino_emulation.h @@ -16,6 +16,7 @@ struct SerialPatch { } void begin(int baudrate); + void flush(); }; extern SerialPatch Serial; diff --git a/MIDAS/src/silsim/emulated_sensors.cpp b/MIDAS/src/silsim/emulated_sensors.cpp index 27c72cf0..4b93d526 100644 --- a/MIDAS/src/silsim/emulated_sensors.cpp +++ b/MIDAS/src/silsim/emulated_sensors.cpp @@ -1,88 +1,91 @@ -#include "silsim/emulated_sensors.h" -#include +#include "emulated_sensors.h" - -ErrorCode BarometerSensor::init() { +ErrorCode EmulatedHw::init_all() { return ErrorCode::NoError; } -Barometer BarometerSensor::read() { - return { 273.15, 0, (float) rocket->height }; +LowGData EmulatedHw::read_low_g() { + return lowGData; } -ErrorCode LowGLSMSensor::init() { - return ErrorCode::NoError; +LowGLSMData EmulatedHw::read_low_g_lsm() { + return lowGlsmData; } -LowGLSM LowGLSMSensor::read() { - return { .gx = 0, .gy = 0, .gz = 0, .ax = 0, .ay = 0, .az =0 }; +HighGData EmulatedHw::read_high_g() { + return highGData; } -ErrorCode ContinuitySensor::init() { - return ErrorCode::NoError; +BarometerData EmulatedHw::read_barometer() { + return barometerData; } -Continuity ContinuitySensor::read() { - return { }; +ContinuityData EmulatedHw::read_continuity() { + return continuityData; } -ErrorCode HighGSensor::init() { - return ErrorCode::NoError; +VoltageData EmulatedHw::read_voltage() { + return voltageData; } -HighGData HighGSensor::read() { - return { 0, 0, (float) rocket->acceleration }; +bool EmulatedHw::is_orientation_ready() { + return true; } -ErrorCode LowGSensor::init() { - return ErrorCode::NoError; +OrientationData EmulatedHw::read_orientation() { + return orientationData; } -LowGData LowGSensor::read() { - return { 0, 0, (float) rocket->acceleration }; +MagnetometerData EmulatedHw::read_magnetometer() { + return magnetometerData; } -ErrorCode OrientationSensor::init() { - return ErrorCode::NoError; +bool EmulatedHw::is_gps_ready() { + return true; } -Orientation OrientationSensor::read() { - return { - .has_data = true, - .yaw = 0, - .pitch = 0, - .roll = 0, - .orientation_velocity = { .vx = 0, .vy = 0, .vz = (float) rocket->velocity }, - .orientation_acceleration = { .ax = 0, .ay = 0, .az = 0 }, - .linear_acceleration = { .ax = 0, .ay = 0, .az = (float) rocket->acceleration }, - .gx = 0, - .gy = 0, - .gz = 0, // todo I don't know what the g's are - .magnetometer = { .mx = 0, .my = 0, .mz = 0}, - .temperature = 273.15, - }; -} - -ErrorCode VoltageSensor::init() { - return ErrorCode::NoError; +GPSData EmulatedHw::read_gps() { + return gpsData; } -Voltage VoltageSensor::read() { - return { .voltage = 9 }; +void EmulatedHw::set_led(LED which, bool value) { + return; } -ErrorCode MagnetometerSensor::init() { - return ErrorCode::NoError; +void EmulatedHw::transmit_bytes(uint8_t* memory, size_t count) { + return; } -Magnetometer MagnetometerSensor::read() { - return { .mx = 0, .my = 0, .mz = 0 }; +bool EmulatedHw::receive_bytes(uint8_t* memory, size_t count, int wait_milliseconds) { + if (pending_transmissions.size() < count) { + return false; + } else { + memcpy(memory, pending_transmissions.data(), count); + pending_transmissions.erase(pending_transmissions.begin(), pending_transmissions.begin()+count); + return true; + } } -ErrorCode GPSSensor::init() { - return ErrorCode::NoError; +void EmulatedHw::set_global_arm(bool to_high) { + return; +} + +void EmulatedHw::set_pin_firing(Channel which, bool to_high) { + return; +} + +void EmulatedHw::set_camera_on(Camera which, bool on) { + return; +} + +void EmulatedHw::set_camera_source(Camera which) { + return; +} + +void EmulatedHw::set_video_transmit(bool on) { + return; } -GPS GPSSensor::read() { - return { }; +uint8_t EmulatedHw::get_camera_state() { + return 0xFF; } diff --git a/MIDAS/src/silsim/emulated_sensors.h b/MIDAS/src/silsim/emulated_sensors.h index 0d295601..0c637f9b 100644 --- a/MIDAS/src/silsim/emulated_sensors.h +++ b/MIDAS/src/silsim/emulated_sensors.h @@ -1,70 +1,44 @@ #pragma once -#include "errors.h" -#include "sensor_data.h" - -#include "silsim/simulation/simulation.h" - - -struct LowGSensor { - ErrorCode init(); - LowGData read(); - - SimulatedRocket* rocket; -}; - -struct LowGLSMSensor { - ErrorCode init(); - LowGLSM read(); - - SimulatedRocket* rocket; -}; - -struct HighGSensor { - ErrorCode init(); - HighGData read(); - - SimulatedRocket* rocket; -}; - -struct BarometerSensor { - ErrorCode init(); - Barometer read(); - - SimulatedRocket* rocket; -}; - -struct ContinuitySensor { - ErrorCode init(); - Continuity read(); - - bool should_be_continous; -}; - -struct VoltageSensor { - ErrorCode init(); - Voltage read(); - - SimulatedRocket* rocket; -}; - -struct MagnetometerSensor { - ErrorCode init(); - Magnetometer read(); - - SimulatedRocket* rocket; -}; - -struct OrientationSensor { - ErrorCode init(); - Orientation read(); - - SimulatedRocket* rocket; -}; - -struct GPSSensor { - ErrorCode init(); - GPS read(); - - SimulatedRocket* rocket; +#include "hal.h" + +struct EmulatedHw : HwInterface { + ErrorCode init_all(); + + LowGData read_low_g(); + LowGLSMData read_low_g_lsm(); + HighGData read_high_g(); + BarometerData read_barometer(); + ContinuityData read_continuity(); + VoltageData read_voltage(); + bool is_orientation_ready(); + OrientationData read_orientation(); + MagnetometerData read_magnetometer(); + bool is_gps_ready(); + GPSData read_gps(); + + void set_led(LED which, bool value); + + void transmit_bytes(uint8_t* memory, size_t count); + bool receive_bytes(uint8_t* memory, size_t count, int wait_milliseconds); + + void set_global_arm(bool to_high); + void set_pin_firing(Channel which, bool to_high); + + void set_camera_on(Camera which, bool on); + void set_camera_source(Camera which); + void set_video_transmit(bool on); + uint8_t get_camera_state(); + + LowGData lowGData; + LowGLSMData lowGlsmData; + HighGData highGData; + BarometerData barometerData; + ContinuityData continuityData; + VoltageData voltageData; + OrientationData orientationData; + MagnetometerData magnetometerData; + GPSData gpsData; + + std::vector pending_transmissions; }; diff --git a/MIDAS/src/silsim/emulated_telemetry.cpp b/MIDAS/src/silsim/emulated_telemetry.cpp deleted file mode 100644 index 16f05cbe..00000000 --- a/MIDAS/src/silsim/emulated_telemetry.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "emulated_telemetry.h" - - -TelemetryBackend::TelemetryBackend(const char* file_name) { - output_file.open(file_name, std::ios::out | std::ios::binary | std::ios::trunc); -} - -ErrorCode __attribute__((warn_unused_result)) TelemetryBackend::init() { - return ErrorCode::NoError; -} - -int16_t TelemetryBackend::getRecentRssi() { - return 1; -} - -void TelemetryBackend::setFrequency(float frequency) { - (void) frequency; -} \ No newline at end of file diff --git a/MIDAS/src/silsim/emulated_telemetry.h b/MIDAS/src/silsim/emulated_telemetry.h deleted file mode 100644 index 60644166..00000000 --- a/MIDAS/src/silsim/emulated_telemetry.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include - -#include "errors.h" -#include "hal.h" - - -class TelemetryBackend { -public: - explicit TelemetryBackend(const char* file_name); - ErrorCode __attribute__((warn_unused_result)) init(); - - int16_t getRecentRssi(); - void setFrequency(float frequency); - - template - void send(const T& data) { - output_file.write((const char*) &data, sizeof(T)); - } - - template - bool read(T* write) { - return false; - } - -private: - std::ofstream output_file; -}; \ No newline at end of file diff --git a/MIDAS/src/silsim/emulation.h b/MIDAS/src/silsim/emulation.h index fa1acc15..be144577 100644 --- a/MIDAS/src/silsim/emulation.h +++ b/MIDAS/src/silsim/emulation.h @@ -1,5 +1,6 @@ #pragma once +#define _USE_MATH_DEFINES #include #include #include @@ -55,6 +56,7 @@ struct SemaphoreHandle_s { typedef SemaphoreHandle_s* SemaphoreHandle_t; typedef size_t TickType_t; +#define portMAX_DELAY ((TickType_t) -1) SemaphoreHandle_t xSemaphoreCreateMutexStatic(StaticSemaphore_t* buffer); bool xSemaphoreTake(SemaphoreHandle_t semaphore, TickType_t timeout); @@ -71,7 +73,7 @@ class StaticQueue_t { return this; } - void push(void* item) { + bool push(void* item) { std::memcpy(&buffer[tail_idx * item_size], item, item_size); tail_idx++; if (tail_idx == max_count) { @@ -85,6 +87,7 @@ class StaticQueue_t { } else { count++; } + return true; } bool pop(void* item) { @@ -115,6 +118,8 @@ typedef StaticQueue_t* QueueHandle_t; #define xQueueSendToBack(queue, value_ptr, timeout) ((queue)->push(value_ptr)) #define xQueueReceive(queue, store_ptr, timeout) ((queue)->pop(store_ptr)) +#define errQUEUE_FULL (false) + void vTaskDelay(int32_t time); void vTaskDelete(void* something_probably); diff --git a/MIDAS/src/silsim/main.cpp b/MIDAS/src/silsim/main.cpp index 3d552c39..d7254494 100644 --- a/MIDAS/src/silsim/main.cpp +++ b/MIDAS/src/silsim/main.cpp @@ -1,71 +1,51 @@ #include "silsim/emulation.h" #include "systems.h" #include "FileSink.h" +#include "emulated_sensors.h" -void rocket_main_thread(RocketSystems* systems) { - begin_systems(systems); -} - -Sensors create_sensors_attached_to(SimulatedRocket* sim, bool should_be_continuous) { - return Sensors { - LowGSensor { sim }, - LowGLSMSensor { sim }, - HighGSensor { sim }, - BarometerSensor { sim }, - ContinuitySensor { should_be_continuous }, - VoltageSensor { sim }, - OrientationSensor { sim }, - MagnetometerSensor { sim }, - }; -} - -RocketSystems* create_and_start(SimulatedRocket* attached, const char* sink_name) { +RocketSystems* create_and_start(EmulatedHw& source, const char* sink_name) { SDSink* sink = new SDSink(sink_name); - RocketSystems* systems = new RocketSystems { - create_sensors_attached_to(attached, true), - {}, - *sink - }; + RocketSystems* systems = new RocketSystems(source, *sink); uint8_t* stack = new uint8_t[4096]; - xTaskCreateStaticPinnedToCore((TaskFunction_t) rocket_main_thread, sink_name, 4096, systems, tskIDLE_PRIORITY, stack, nullptr, 0); + xTaskCreateStaticPinnedToCore((TaskFunction_t) [](void* arg) { ((RocketSystems*) arg)->begin(); }, sink_name, 4096, systems, tskIDLE_PRIORITY, stack, nullptr, 0); return systems; } int main() { begin_silsim(); - SimulatedRocket* together_sim = new SimulatedRocket(true, RocketParameters(1, 1, 0.75, SimulatedMotor(100.0, 10.0))); - SimulatedRocket* stage1_sim = new SimulatedRocket(false, RocketParameters(0.5, 1, 0.75, SimulatedMotor(100.0, 10.0))); - SimulatedRocket* stage2_sim = new SimulatedRocket(false, RocketParameters(0.5, 1, 0.75, SimulatedMotor(0.0, 10.0))); - - Simulation sim({ .density_of_air = 0.001, .gravity = 9.81 }, std::vector { together_sim, stage1_sim, stage2_sim }); - - RocketSystems* stage1_systems = create_and_start(together_sim, "stage1_sink.launch"); - RocketSystems* stage2_systems = create_and_start(together_sim, "stage2_sink.launch"); - - sim.ignite_rocket(together_sim); - - while (!stage1_systems->rocket_data.pyro_should_be_firing && (together_sim->height != 0 || sim.current_time < 0.1)) { - sim.step(0.001); - - silsimStepTime(); - - std::cout << "Rocket altitude: " << together_sim->height << " velocity: " << together_sim->velocity << std::endl; - } - - stage1_sim->copy_state_from(together_sim); - stage2_sim->copy_state_from(together_sim); - sim.activate_rocket(stage1_sim); - sim.activate_rocket(stage2_sim); - sim.deactivate_rocket(together_sim); - sim.ignite_rocket(stage2_sim); - stage1_systems->sensors = create_sensors_attached_to(stage1_sim, false); - stage2_systems->sensors = create_sensors_attached_to(stage2_sim, false); - - while (stage2_sim->height != 0 && stage1_sim->height != 0) { - sim.step(0.001); - - silsimStepTime(); - } +// SimulatedRocket* together_sim = new SimulatedRocket(true, RocketParameters(1, 1, 0.75, SimulatedMotor(100.0, 10.0))); +// SimulatedRocket* stage1_sim = new SimulatedRocket(false, RocketParameters(0.5, 1, 0.75, SimulatedMotor(100.0, 10.0))); +// SimulatedRocket* stage2_sim = new SimulatedRocket(false, RocketParameters(0.5, 1, 0.75, SimulatedMotor(0.0, 10.0))); + +// Simulation sim({ .density_of_air = 0.001, .gravity = 9.81 }, std::vector { together_sim, stage1_sim, stage2_sim }); + +// RocketSystems* stage1_systems = create_and_start(together_sim, "stage1_sink.launch"); +// RocketSystems* stage2_systems = create_and_start(together_sim, "stage2_sink.launch"); +// +// sim.ignite_rocket(together_sim); +// +// while (!stage1_systems->rocket_data.pyro_should_be_firing && (together_sim->height != 0 || sim.current_time < 0.1)) { +// sim.step(0.001); +// +// silsimStepTime(); +// +// std::cout << "Rocket altitude: " << together_sim->height << " velocity: " << together_sim->velocity << std::endl; +// } + +// stage1_sim->copy_state_from(together_sim); +// stage2_sim->copy_state_from(together_sim); +// sim.activate_rocket(stage1_sim); +// sim.activate_rocket(stage2_sim); +// sim.deactivate_rocket(together_sim); +// sim.ignite_rocket(stage2_sim); +// stage1_systems->sensors = create_sensors_attached_to(stage1_sim, false); +// stage2_systems->sensors = create_sensors_attached_to(stage2_sim, false); + +// while (stage2_sim->height != 0 && stage1_sim->height != 0) { +// sim.step(0.001); +// +// silsimStepTime(); +// } } diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp deleted file mode 100644 index f2e9b53c..00000000 --- a/MIDAS/src/systems.cpp +++ /dev/null @@ -1,423 +0,0 @@ -#include "systems.h" - -#include "hal.h" -#include "gnc/ekf.h" - -#include - -#if defined(IS_SUSTAINER) && defined(IS_BOOSTER) -#error "Only one of IS_SUSTAINER and IS_BOOSTER may be defined at the same time." -#elif !defined(IS_SUSTAINER) && !defined(IS_BOOSTER) -#error "At least one of IS_SUSTAINER and IS_BOOSTER must be defined." -#endif - -#define ENABLE_TELEM - -/** - * @brief These are all the functions that will run in each task - * Each function has a `while (true)` loop within that should not be returned out of or yielded in any way - * - * The `DECLARE_THREAD` macro creates a function whose name is suffixed by _thread, and annotates it with [[noreturn]] - */ -DECLARE_THREAD(logger, RocketSystems* arg) { - log_begin(arg->log_sink); - while (true) { - log_data(arg->log_sink, arg->rocket_data); - - arg->rocket_data.log_latency.tick(); - - THREAD_SLEEP(1); - } -} - -DECLARE_THREAD(barometer, RocketSystems* arg) { - // Reject single rogue barometer readings that are very different from the immediately prior reading - // Will only reject a certain number of readings in a row - Barometer prev_reading; - constexpr float altChgThreshold = 200; // meters - constexpr float presChgThreshold = 500; // milibars - constexpr float tempChgThreshold = 10; // degrees C - constexpr unsigned int maxConsecutiveRejects = 3; - unsigned int rejects = maxConsecutiveRejects; // Always accept first reading - while (true) { - Barometer reading = arg->sensors.barometer.read(); - bool is_rogue = std::abs(prev_reading.altitude - reading.altitude) > altChgThreshold; - //std::abs(prev_reading.pressure - reading.pressure) > presChgThreshold || - //std::abs(prev_reading.temperature - reading.temperature) > tempChgThreshold; - // TODO: Log when we receive a rejection! - if (is_rogue && rejects++ < maxConsecutiveRejects) - arg->rocket_data.barometer.update(prev_reading); // Reuse old reading, reject new reading - else { - rejects = 0; - arg->rocket_data.barometer.update(reading); - prev_reading = reading; // Only update prev_reading with accepted readings - } - // Serial.print("Barometer "); - // Serial.print(reading.altitude); - // Serial.print(" "); - // Serial.print(reading.pressure); - // Serial.print(" "); - // Serial.println(reading.temperature); - THREAD_SLEEP(6); - } -} - -DECLARE_THREAD(accelerometers, RocketSystems* arg) { - while (true) { - LowGData lowg = arg->sensors.low_g.read(); - arg->rocket_data.low_g.update(lowg); - LowGLSM lowglsm = arg->sensors.low_g_lsm.read(); - arg->rocket_data.low_g_lsm.update(lowglsm); - HighGData highg = arg->sensors.high_g.read(); - arg->rocket_data.high_g.update(highg); - - THREAD_SLEEP(2); - } -} - -DECLARE_THREAD(orientation, RocketSystems* arg) { - while (true) { - Orientation orientation_holder = arg->rocket_data.orientation.getRecent(); - Orientation reading = arg->sensors.orientation.read(); - if (reading.has_data) { - if(reading.reading_type == OrientationReadingType::ANGULAR_VELOCITY_UPDATE) { - orientation_holder.angular_velocity.vx = reading.angular_velocity.vx; - orientation_holder.angular_velocity.vy = reading.angular_velocity.vy; - orientation_holder.angular_velocity.vz = reading.angular_velocity.vz; - } else { - float old_vx = orientation_holder.angular_velocity.vx; - float old_vy = orientation_holder.angular_velocity.vy; - float old_vz = orientation_holder.angular_velocity.vz; - orientation_holder = reading; - orientation_holder.angular_velocity.vx = old_vx; - orientation_holder.angular_velocity.vy = old_vy; - orientation_holder.angular_velocity.vz = old_vz; - } - - arg->rocket_data.orientation.update(orientation_holder); - } - - THREAD_SLEEP(100); - } -} - -DECLARE_THREAD(magnetometer, RocketSystems* arg) { - while (true) { - Magnetometer reading = arg->sensors.magnetometer.read(); - arg->rocket_data.magnetometer.update(reading); - THREAD_SLEEP(50); - } -} - -DECLARE_THREAD(gps, RocketSystems* arg) { - while(true) { - if(arg->sensors.gps.valid()) { - GPS reading = arg->sensors.gps.read(); - arg->rocket_data.gps.update(reading); - } - //GPS waits internally - THREAD_SLEEP(1); - } -} - -DECLARE_THREAD(pyro, RocketSystems* arg) { - while(true) { - FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); - CommandFlags& command_flags = arg->rocket_data.command_flags; - - PyroState new_pyro_state = arg->sensors.pyro.tick(current_state, arg->rocket_data.orientation.getRecentUnsync(), command_flags); - arg->rocket_data.pyro.update(new_pyro_state); - - arg->led.update(); - - THREAD_SLEEP(10); - } -} - -DECLARE_THREAD(voltage, RocketSystems* arg) { - while (true) { - Continuity reading = arg->sensors.continuity.read(); - Voltage reading2 = arg->sensors.voltage.read();; - - arg->rocket_data.continuity.update(reading); - arg->rocket_data.voltage.update(reading2); - - THREAD_SLEEP(100); - } -} - -// This thread has a bit of extra logic since it needs to play a tune exactly once the sustainer ignites -DECLARE_THREAD(fsm, RocketSystems* arg) { - FSM fsm{}; - bool already_played_freebird = false; - double last_time_led_flash = pdTICKS_TO_MS(xTaskGetTickCount()); - while (true) { - FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); - StateEstimate state_estimate(arg->rocket_data); - CommandFlags& telemetry_commands = arg->rocket_data.command_flags; - double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - - FSMState next_state = fsm.tick_fsm(current_state, state_estimate, telemetry_commands); - - arg->rocket_data.fsm_state.update(next_state); - - if (current_state == FSMState::STATE_SAFE) { - if((current_time - last_time_led_flash) > 250) { - // Flashes green LED at 4Hz while in SAFE mode. - last_time_led_flash = current_time; - arg->led.toggle(LED::GREEN); - } - } else { - arg->led.set(LED::GREEN, LOW); - } - - if ((current_state == FSMState::STATE_PYRO_TEST || current_state == FSMState::STATE_IDLE) && !arg->buzzer.is_playing()) { - arg->buzzer.play_tune(warn_tone, WARN_TONE_LENGTH); - } - - if (current_state == FSMState::STATE_LANDED && !arg->buzzer.is_playing()) { - arg->buzzer.play_tune(land_tone, LAND_TONE_LENGTH); - } - - if (current_state == FSMState::STATE_SUSTAINER_IGNITION && !already_played_freebird) { - arg->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); - already_played_freebird = true; - } - - // FSM-based camera control - if(arg->rocket_data.command_flags.FSM_should_set_cam_feed_cam1) { - // Swap camera feed to MUX 1 (Side-facing camera) at launch. - arg->rocket_data.command_flags.FSM_should_set_cam_feed_cam1 = false; - arg->b2b.camera.vmux_set(SIDE_CAMERA); - } - - if(arg->rocket_data.command_flags.FSM_should_swap_camera_feed) { - // Swap camera feed to MUX 2 (recovery bay camera) - arg->rocket_data.command_flags.FSM_should_swap_camera_feed = false; - arg->b2b.camera.vmux_set(BULKHEAD_CAMERA); - } - - THREAD_SLEEP(50); - } -} - -DECLARE_THREAD(buzzer, RocketSystems* arg) { - while (true) { - arg->buzzer.tick(); - - THREAD_SLEEP(10); - } -} - -DECLARE_THREAD(kalman, RocketSystems* arg) { - ekf.initialize(arg); - // Serial.println("Initialized ekf :("); - TickType_t last = xTaskGetTickCount(); - - while (true) { - if(arg->rocket_data.command_flags.should_reset_kf){ - ekf.initialize(arg); - TickType_t last = xTaskGetTickCount(); - arg->rocket_data.command_flags.should_reset_kf = false; - } - // add the tick update function - Barometer current_barom_buf = arg->rocket_data.barometer.getRecent(); - Orientation current_orientation = arg->rocket_data.orientation.getRecent(); - HighGData current_accelerometer = arg->rocket_data.high_g.getRecent(); - FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); - Acceleration current_accelerations = { - .ax = current_accelerometer.ax, - .ay = current_accelerometer.ay, - .az = current_accelerometer.az - }; - float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; - float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); - KalmanData current_state = ekf.getState(); - - arg->rocket_data.kalman.update(current_state); - - last = xTaskGetTickCount(); - // Serial.println("Kalman"); - THREAD_SLEEP(50); - } -} - - -void handle_tlm_command(TelemetryCommand& command, RocketSystems* arg, FSMState current_state) { - // maybe we should move this somewhere else but it can stay here for now - switch(command.command) { - case CommandType::RESET_KF: - arg->rocket_data.command_flags.should_reset_kf = true; - break; - case CommandType::SWITCH_TO_SAFE: - arg->rocket_data.command_flags.should_transition_safe = true; - break; - case CommandType::SWITCH_TO_PYRO_TEST: - arg->rocket_data.command_flags.should_transition_pyro_test = true; - Serial.println("Changing to pyro test"); - break; - case CommandType::SWITCH_TO_IDLE: - arg->rocket_data.command_flags.should_transition_idle = true; - break; - case CommandType::FIRE_PYRO_A: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_a = true; - } - break; - case CommandType::FIRE_PYRO_B: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_b = true; - } - break; - case CommandType::FIRE_PYRO_C: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_c = true; - } - break; - case CommandType::FIRE_PYRO_D: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_d = true; - } - break; - case CommandType::CAM_ON: - arg->b2b.camera.camera_on(CAM_1); - arg->b2b.camera.camera_on(CAM_2); - arg->b2b.camera.vtx_on(); - break; - case CommandType::CAM_OFF: - arg->b2b.camera.camera_off(CAM_1); - arg->b2b.camera.camera_off(CAM_2); - arg->b2b.camera.vtx_off(); - break; - case CommandType::TOGGLE_CAM_VMUX: - arg->b2b.camera.vmux_toggle(); - break; - default: - break; // how - } -} - -DECLARE_THREAD(cam, RocketSystems* arg) { - while (true) { - arg->rocket_data.camera_state = arg->b2b.camera.read(); - THREAD_SLEEP(200); - } -} - -DECLARE_THREAD(telemetry, RocketSystems* arg) { - double launch_time = 0; - bool has_triggered_vmux_fallback = false; - - while (true) { - - arg->tlm.transmit(arg->rocket_data, arg->led); - - FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); - double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - - if (current_state == FSMState::STATE_IDLE) { - launch_time = current_time; - has_triggered_vmux_fallback = false; - } - - if ((current_time - launch_time) > 80000 && !has_triggered_vmux_fallback) { - // THIS IS A HARDCODED VALUE FOR AETHER 3/15/2025 - // If the rocket has been in flight for over 80 seconds, we swap the FSM camera feed to the bulkhead camera - has_triggered_vmux_fallback = true; - arg->rocket_data.command_flags.FSM_should_swap_camera_feed = true; - } - - if (current_state == FSMState(STATE_IDLE) || current_state == FSMState(STATE_SAFE) || current_state == FSMState(STATE_PYRO_TEST) || (current_time - launch_time) > 1800000) { - TelemetryCommand command; - if (arg->tlm.receive(&command, 200)) { - if (command.valid()) { - arg->tlm.acknowledgeReceived(); - handle_tlm_command(command, arg, current_state); - } - } - } - THREAD_SLEEP(1); - } -} - -#define INIT_SYSTEM(s) do { ErrorCode code = (s).init(); if (code != NoError) { return code; } } while (0) - -/** - * @brief Initializes all systems in order, returning early if a system's initialization process errors out. - * Turns on the Orange LED while initialization is running. - */ -ErrorCode init_systems(RocketSystems& systems) { - gpioDigitalWrite(LED_ORANGE, HIGH); - INIT_SYSTEM(systems.sensors.low_g); - INIT_SYSTEM(systems.sensors.orientation); - INIT_SYSTEM(systems.log_sink); - INIT_SYSTEM(systems.sensors.high_g); - INIT_SYSTEM(systems.sensors.low_g_lsm); - INIT_SYSTEM(systems.sensors.barometer); - INIT_SYSTEM(systems.sensors.magnetometer); - INIT_SYSTEM(systems.sensors.continuity); - INIT_SYSTEM(systems.sensors.voltage); - INIT_SYSTEM(systems.sensors.pyro); - INIT_SYSTEM(systems.led); - INIT_SYSTEM(systems.buzzer); - INIT_SYSTEM(systems.b2b); - #ifdef ENABLE_TELEM - INIT_SYSTEM(systems.tlm); - #endif - INIT_SYSTEM(systems.sensors.gps); - gpioDigitalWrite(LED_ORANGE, LOW); - return NoError; -} -#undef INIT_SYSTEM - - -/** - * @brief Initializes the systems, and then creates and starts the thread for each system. - * If initialization fails, then this enters an infinite loop. - */ -[[noreturn]] void begin_systems(RocketSystems* config) { - Serial.println("Starting Systems..."); - ErrorCode init_error_code = init_systems(*config); - if (init_error_code != NoError) { - // todo some message probably - Serial.print("Had Error: "); - Serial.print((int) init_error_code); - Serial.print("\n"); - Serial.flush(); - update_error_LED(init_error_code); - while (true) { - - } - } - - START_THREAD(orientation, SENSOR_CORE, config, 10); - START_THREAD(logger, DATA_CORE, config, 15); - START_THREAD(accelerometers, SENSOR_CORE, config, 13); - START_THREAD(barometer, SENSOR_CORE, config, 12); - START_THREAD(gps, SENSOR_CORE, config, 8); - START_THREAD(voltage, SENSOR_CORE, config, 9); - START_THREAD(pyro, SENSOR_CORE, config, 14); - START_THREAD(magnetometer, SENSOR_CORE, config, 11); - START_THREAD(cam, SENSOR_CORE, config, 16); - START_THREAD(kalman, SENSOR_CORE, config, 7); - START_THREAD(fsm, SENSOR_CORE, config, 8); - START_THREAD(buzzer, SENSOR_CORE, config, 6); - #ifdef ENABLE_TELEM - START_THREAD(telemetry, SENSOR_CORE, config, 15); - #endif - - config->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); - - while (true) { - THREAD_SLEEP(1000); - Serial.print("Running (Log Latency: "); - Serial.print(config->rocket_data.log_latency.getLatency()); - Serial.println(")"); - } -} - -//void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char* pcTaskName){ -// Serial.println("OVERFLOW"); -// Serial.println((char*)pcTaskName); -//} diff --git a/MIDAS/src/systems.h b/MIDAS/src/systems.h index fd9b7b1f..61706298 100644 --- a/MIDAS/src/systems.h +++ b/MIDAS/src/systems.h @@ -1,57 +1,518 @@ #pragma once -#include - -#include "sensor_data.h" -#include "hal.h" -#include "Buffer.h" #include "data_logging.h" #include "buzzer.h" #include "led.h" #include "telemetry.h" #include "finite-state-machines/fsm.h" -#include "b2b_interface.h" - -#if defined(SILSIM) -#include "silsim/emulated_sensors.h" -#elif defined(HILSIM) -#include "TCAL9539.h" -#include "hilsim/sensors.h" -#else -#include "hardware/sensors.h" +#include "gnc/ekf.h" +#include "pyro.h" + +#define ENABLE_TELEM + +#if defined(IS_SUSTAINER) && defined(IS_BOOSTER) +#error "Only one of IS_SUSTAINER and IS_BOOSTER may be defined at the same time." +#elif !defined(IS_SUSTAINER) && !defined(IS_BOOSTER) +#error "At least one of IS_SUSTAINER and IS_BOOSTER must be defined." #endif /** - * @struct Sensors - * - * @brief holds all interfaces for all sensors on MIDAS -*/ -struct Sensors { - LowGSensor low_g; - LowGLSMSensor low_g_lsm; - HighGSensor high_g; - BarometerSensor barometer; - ContinuitySensor continuity; - VoltageSensor voltage; - OrientationSensor orientation; - MagnetometerSensor magnetometer; - Pyro pyro; - GPSSensor gps; -}; + * The size of a thread stack, in bytes. + */ +#define STACK_SIZE 8192 + +/** + * The ESP32 has two physical cores, which will each be dedicated to one group of threads. + * The SENSOR_CORE runs the threads which write to the sensor_data struct (mostly sensor polling threads). + */ +#define SENSOR_CORE ((BaseType_t) 0) + +/** + * The ESP32 has two physical cores, which will each be dedicated to one group of threads. + * The DATA_CORE runs the GPS thread, as well as the threads which read from the sensor_data struct (e.g. SD logging). + */ +#define DATA_CORE ((BaseType_t) 1) + +/** + * Macro for declaring a thread. Creates a method with the name suffixed with `_thread`, annotated with [[noreturn]]. + * + * @param name The name of the task. + */ +#define DECLARE_THREAD(name) unsigned char name##_stack[STACK_SIZE] = {}; [[noreturn]] void name##_thread() + +#define DEFINE_THREAD(name) template [[noreturn]] void RocketSystems::name##_thread() + +/** + * Macro for creating and starting a thread declared with `DECLARE_THREAD`. This is a statement and has no return value. + * Never put this in a scope that will end before the thread should. + * + * @param name Name of the thread. + * @param core Core for the task to be pinned to, either `SENSOR_CORE` or `DATA_CORE`. + * @param arg Argument passed in to the `param` argument of `DECLARE_THREAD`. + * @param prio Priority of the thread. + */ +#define START_THREAD(name, core, arg, prio) StaticTask_t name##_task; \ + xTaskCreateStaticPinnedToCore(((TaskFunction_t) [](void* sys) { ((RocketSystems*) sys)->name##_thread(); }), #name, STACK_SIZE, arg, tskIDLE_PRIORITY + prio, name##_stack, &name##_task, core) +/* + * Parameters for xTaskCreateStaticPinnedToCore are as follows in parameter order: + * - Function to be run by the thread, this contains a `while(true)` loop + * - Name of thread + * - Size of the stack for each thread in words (1 word = 4 bytes) + * - Arguments to be passed into the function, this will generally eb the config file + * - Priority of the task, in allmost all cases, this will be the idle priority plus one + * - The actual stack memory to use + * - A handle to reference the task with + * - The core to pin the task to + */ + /** * @struct RocketData * * @brief holds all information about the rocket, sensors, and controllers -*/ + */ +template struct RocketSystems { - Sensors sensors; + HwInterface& hw; RocketData rocket_data; LogSink& log_sink; BuzzerController buzzer; - LEDController led; - Telemetry tlm; - B2BInterface b2b; + LEDController led; + PyroLogic pyro; + Telemetry tlm; + EKF ekf; + + bool video_toggle = false; + + RocketSystems(HwInterface& hw, LogSink& log_sink) : hw(hw), log_sink(log_sink), pyro(hw), led(hw), tlm(hw, led) { } + + ErrorCode init_systems(); + [[noreturn]] void begin(); + + void handle_tlm_command(TelemetryCommand& command, FSMState current_state); + + DECLARE_THREAD(logger); + DECLARE_THREAD(barometer); + DECLARE_THREAD(accelerometers); + DECLARE_THREAD(orientation); + DECLARE_THREAD(magnetometer); + DECLARE_THREAD(gps); + DECLARE_THREAD(pyro); + DECLARE_THREAD(voltage); + DECLARE_THREAD(fsm); + DECLARE_THREAD(buzzer); + DECLARE_THREAD(kalman); + DECLARE_THREAD(cam); + DECLARE_THREAD(telemetry); }; -[[noreturn]] void begin_systems(RocketSystems* config); +/** + * @brief These are all the functions that will run in each task + * Each function has a `while (true)` loop within that should not be returned out of or yielded in any way + * + * The `DECLARE_THREAD` macro creates a function whose name is suffixed by _thread, and annotates it with [[noreturn]] + */ + +DEFINE_THREAD(logger) { + log_begin(log_sink); + while (true) { + log_data(log_sink, rocket_data); + + rocket_data.log_latency.tick(); + + THREAD_SLEEP(1); + } +} + +DEFINE_THREAD(barometer) { + // Reject single rogue barometer readings that are very different from the immediately prior reading + // Will only reject a certain number of readings in a row + BarometerData prev_reading; + constexpr float altChgThreshold = 200; // meters + constexpr float presChgThreshold = 500; // milibars + constexpr float tempChgThreshold = 10; // degrees C + constexpr unsigned int maxConsecutiveRejects = 3; + unsigned int rejects = maxConsecutiveRejects; // Always accept first reading + while (true) { + BarometerData reading = hw.read_barometer(); + bool is_rogue = std::abs(prev_reading.altitude - reading.altitude) > altChgThreshold; + //std::abs(prev_reading.pressure - reading.pressure) > presChgThreshold || + //std::abs(prev_reading.temperature - reading.temperature) > tempChgThreshold; + // TODO: Log when we receive a rejection! + if (is_rogue && rejects++ < maxConsecutiveRejects) + rocket_data.barometer.update(prev_reading); // Reuse old reading, reject new reading + else { + rejects = 0; + rocket_data.barometer.update(reading); + prev_reading = reading; // Only update prev_reading with accepted readings + } + // Serial.print("Barometer "); + // Serial.print(reading.altitude); + // Serial.print(" "); + // Serial.print(reading.pressure); + // Serial.print(" "); + // Serial.println(reading.temperature); + THREAD_SLEEP(6); + } +} + +DEFINE_THREAD(accelerometers) { + while (true) { + LowGData lowg = hw.read_low_g(); + rocket_data.low_g.update(lowg); + LowGLSMData lowglsm = hw.read_low_g_lsm(); + rocket_data.low_g_lsm.update(lowglsm); + HighGData highg = hw.read_high_g(); + rocket_data.high_g.update(highg); + + THREAD_SLEEP(2); + } +} + +DEFINE_THREAD(orientation) { + while (true) { + OrientationData orientation_holder = rocket_data.orientation.getRecent(); + OrientationData reading = hw.read_orientation(); + if (reading.has_data) { + if (reading.reading_type == OrientationReadingType::ANGULAR_VELOCITY_UPDATE) { + orientation_holder.angular_velocity.vx = reading.angular_velocity.vx; + orientation_holder.angular_velocity.vy = reading.angular_velocity.vy; + orientation_holder.angular_velocity.vz = reading.angular_velocity.vz; + } else { + float old_vx = orientation_holder.angular_velocity.vx; + float old_vy = orientation_holder.angular_velocity.vy; + float old_vz = orientation_holder.angular_velocity.vz; + orientation_holder = reading; + orientation_holder.angular_velocity.vx = old_vx; + orientation_holder.angular_velocity.vy = old_vy; + orientation_holder.angular_velocity.vz = old_vz; + } + + rocket_data.orientation.update(orientation_holder); + } + + THREAD_SLEEP(100); + } +} + +DEFINE_THREAD(magnetometer) { + while (true) { + MagnetometerData reading = hw.read_magnetometer(); + rocket_data.magnetometer.update(reading); + THREAD_SLEEP(50); + } +} + +DEFINE_THREAD(gps) { + while (true) { + if (hw.is_gps_ready()) { + GPSData reading = hw.read_gps(); + rocket_data.gps.update(reading); + } + //GPS waits internally + THREAD_SLEEP(1); + } +} + +DEFINE_THREAD(pyro) { + while (true) { + FSMState current_state = rocket_data.fsm_state.getRecentUnsync(); + CommandFlags& command_flags = rocket_data.command_flags; + + PyroState new_pyro_state = pyro.tick(current_state, rocket_data.orientation.getRecentUnsync(), command_flags); + rocket_data.pyro.update(new_pyro_state); + + led.update(); + + THREAD_SLEEP(10); + } +} + +DEFINE_THREAD(voltage) { + while (true) { + ContinuityData reading = hw.read_continuity(); + VoltageData reading2 = hw.read_voltage(); + + rocket_data.continuity.update(reading); + rocket_data.voltage.update(reading2); + + THREAD_SLEEP(100); + } +} + +// This thread has a bit of extra logic since it needs to play a tune exactly once the sustainer ignites +DEFINE_THREAD(fsm) { + FSM fsm {}; + bool already_played_freebird = false; + double last_time_led_flash = pdTICKS_TO_MS(xTaskGetTickCount()); + while (true) { + FSMState current_state = rocket_data.fsm_state.getRecentUnsync(); + StateEstimate state_estimate(rocket_data); + CommandFlags& telemetry_commands = rocket_data.command_flags; + double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); + + FSMState next_state = fsm.tick_fsm(current_state, state_estimate, telemetry_commands); + + rocket_data.fsm_state.update(next_state); + + if (current_state == FSMState::STATE_SAFE) { + if ((current_time - last_time_led_flash) > 250) { + // Flashes green LED at 4Hz while in SAFE mode. + last_time_led_flash = current_time; + led.toggle(LED::GREEN); + } + } else { + led.set(LED::GREEN, LOW); + } + + if ((current_state == FSMState::STATE_PYRO_TEST || current_state == FSMState::STATE_IDLE) && + !buzzer.is_playing()) { + buzzer.play_tune(warn_tone, WARN_TONE_LENGTH); + } + + if (current_state == FSMState::STATE_LANDED && !buzzer.is_playing()) { + buzzer.play_tune(land_tone, LAND_TONE_LENGTH); + } + + if (current_state == FSMState::STATE_SUSTAINER_IGNITION && !already_played_freebird) { + buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); + already_played_freebird = true; + } + + // FSM-based camera control + if (rocket_data.command_flags.FSM_should_set_cam_feed_cam1) { + // Swap camera feed to MUX 1 (Side-facing camera) at launch. + rocket_data.command_flags.FSM_should_set_cam_feed_cam1 = false; + hw.set_camera_source(Camera::Side); + } + + if (rocket_data.command_flags.FSM_should_swap_camera_feed) { + // Swap camera feed to MUX 2 (recovery bay camera) + rocket_data.command_flags.FSM_should_swap_camera_feed = false; + hw.set_camera_source(Camera::Bulkhead); + } + + THREAD_SLEEP(50); + } +} + +DEFINE_THREAD(buzzer) { + while (true) { + buzzer.tick(); + + THREAD_SLEEP(10); + } +} + +DEFINE_THREAD(kalman) { + ekf.initialize(rocket_data); + TickType_t last = xTaskGetTickCount(); + // Serial.println("Initialized ekf :("); + + while (true) { + if (rocket_data.command_flags.should_reset_kf) { + ekf.initialize(rocket_data); + last = xTaskGetTickCount(); + rocket_data.command_flags.should_reset_kf = false; + } + + // add the tick update function + BarometerData current_barom_buf = rocket_data.barometer.getRecent(); + OrientationData current_orientation = rocket_data.orientation.getRecent(); + HighGData current_accelerometer = rocket_data.high_g.getRecent(); + FSMState FSM_state = rocket_data.fsm_state.getRecent(); + Acceleration current_accelerations = { + .ax = current_accelerometer.ax, + .ay = current_accelerometer.ay, + .az = current_accelerometer.az + }; + + TickType_t now = xTaskGetTickCount(); + float dt = pdTICKS_TO_MS(now - last) / 1000.0f; + last = now; + + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + KalmanData current_state = ekf.getState(); + + rocket_data.kalman.update(current_state); + + // Serial.println("Kalman"); + THREAD_SLEEP(50); + } +} + +template +void RocketSystems::handle_tlm_command(TelemetryCommand& command, FSMState current_state) { + // maybe we should move this somewhere else but it can stay here for now + switch (command.command) { + case CommandType::RESET_KF: { + rocket_data.command_flags.should_reset_kf = true; + break; + } + case CommandType::SWITCH_TO_SAFE: { + rocket_data.command_flags.should_transition_safe = true; + break; + } + case CommandType::SWITCH_TO_PYRO_TEST: { + rocket_data.command_flags.should_transition_pyro_test = true; + Serial.println("Changing to pyro test"); + break; + } + case CommandType::SWITCH_TO_IDLE: { + rocket_data.command_flags.should_transition_idle = true; + break; + } + case CommandType::FIRE_PYRO_A: { + if (current_state == FSMState::STATE_PYRO_TEST) { + rocket_data.command_flags.should_fire_pyro_a = true; + } + break; + } + case CommandType::FIRE_PYRO_B: { + if (current_state == FSMState::STATE_PYRO_TEST) { + rocket_data.command_flags.should_fire_pyro_b = true; + } + break; + } + case CommandType::FIRE_PYRO_C: { + if (current_state == FSMState::STATE_PYRO_TEST) { + rocket_data.command_flags.should_fire_pyro_c = true; + } + break; + } + case CommandType::FIRE_PYRO_D: { + if (current_state == FSMState::STATE_PYRO_TEST) { + rocket_data.command_flags.should_fire_pyro_d = true; + } + break; + } + case CommandType::CAM_ON: { + hw.set_camera_on(Camera::Side, true); + hw.set_camera_on(Camera::Bulkhead, true); + hw.set_video_transmit(true); + break; + } + case CommandType::CAM_OFF: { + hw.set_camera_on(Camera::Side, false); + hw.set_camera_on(Camera::Bulkhead, false); + hw.set_video_transmit(false); + break; + } + case CommandType::TOGGLE_CAM_VMUX: { + hw.set_camera_source(video_toggle ? Camera::Bulkhead : Camera::Side); + video_toggle = !video_toggle; + break; + } + default: { + break; // how + } + } +} + +DEFINE_THREAD(cam) { + while (true) { + rocket_data.camera_state = hw.get_camera_state(); + THREAD_SLEEP(200); + } +} + +DEFINE_THREAD(telemetry) { + double launch_time = 0; + bool has_triggered_vmux_fallback = false; + + while (true) { + tlm.transmit(rocket_data); + + FSMState current_state = rocket_data.fsm_state.getRecentUnsync(); + double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); + + if (current_state == FSMState::STATE_IDLE) { + launch_time = current_time; + has_triggered_vmux_fallback = false; + } + + if ((current_time - launch_time) > 80000 && !has_triggered_vmux_fallback) { + // THIS IS A HARDCODED VALUE FOR AETHER 3/15/2025 + // If the rocket has been in flight for over 80 seconds, we swap the FSM camera feed to the bulkhead camera + has_triggered_vmux_fallback = true; + rocket_data.command_flags.FSM_should_swap_camera_feed = true; + } + + if (current_state == FSMState(STATE_IDLE) || current_state == FSMState(STATE_SAFE) || + current_state == FSMState(STATE_PYRO_TEST) || (current_time - launch_time) > 1800000) { + TelemetryCommand command; + if (tlm.receive(&command, 200)) { + if (command.valid()) { + tlm.acknowledgeReceived(); + handle_tlm_command(command, current_state); + } + } + } + THREAD_SLEEP(1); + } +} + +#define INIT_SYSTEM(s) do { ErrorCode code = (s).init(); if (code != NoError) { return code; } } while (0) + +/** + * @brief Initializes all systems in order, returning early if a system's initialization process errors out. + * Turns on the Orange LED while initialization is running. + */ +template +ErrorCode RocketSystems::init_systems() { + hw.set_led(LED::ORANGE, true); + ErrorCode c = hw.init_all(); + if (c != ErrorCode::NoError) { + return c; + } + INIT_SYSTEM(buzzer); + hw.set_led(LED::ORANGE, false); + return NoError; +} +#undef INIT_SYSTEM + +/** + * @brief Initializes the systems, and then creates and starts the thread for each system. + * If initialization fails, then this enters an infinite loop. + */ +template +[[noreturn]] void RocketSystems::begin() { + Serial.println("Starting Systems..."); + ErrorCode init_error_code = init_systems(); + if (init_error_code != NoError) { + // todo some message probably + Serial.print("Had Error: "); + Serial.print((int) init_error_code); + Serial.print("\n"); + Serial.flush(); + update_error_LED(init_error_code); + while (true) { + + } + } + + START_THREAD(orientation, SENSOR_CORE, this, 10); + START_THREAD(logger, DATA_CORE, this, 15); + START_THREAD(accelerometers, SENSOR_CORE, this, 13); + START_THREAD(barometer, SENSOR_CORE, this, 12); + START_THREAD(gps, SENSOR_CORE, this, 8); + START_THREAD(voltage, SENSOR_CORE, this, 9); + START_THREAD(pyro, SENSOR_CORE, this, 14); + START_THREAD(magnetometer, SENSOR_CORE, this, 11); + START_THREAD(cam, SENSOR_CORE, this, 16); + START_THREAD(kalman, SENSOR_CORE, this, 7); + START_THREAD(fsm, SENSOR_CORE, this, 8); + START_THREAD(buzzer, SENSOR_CORE, this, 6); +#ifdef ENABLE_TELEM + START_THREAD(telemetry, SENSOR_CORE, this, 15); +#endif + + buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); + + while (true) { + THREAD_SLEEP(1000); + Serial.print("Running (Log Latency: "); + Serial.print(rocket_data.log_latency.getLatency()); + Serial.println(")"); + } +} diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index 60aa6735..1230952b 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry.cpp @@ -1,3 +1,4 @@ +#define _USE_MATH_DEFINES #include #include "telemetry.h" @@ -6,7 +7,6 @@ inline long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } - /** * @brief This function maps an input value onto within a particular range into a fixed point value of a certin binary * size @@ -45,50 +45,21 @@ std::tuple pack_highg_tilt(HighGData con return {x,y,z,q}; } -/** - * @brief move constructor for the telemetry backend -*/ -Telemetry::Telemetry(TelemetryBackend&& backend) : backend(std::move(backend)) { } - -/** - * @brief transmit telemetry data through LoRa - * - * @param rocket_data rocket_data to transmit - * @param led led state to transmit -*/ -void Telemetry::transmit(RocketData& rocket_data, LEDController& led) { - // static_assert(sizeof(TelemetryPacket) == 20); - - TelemetryPacket packet = makePacket(rocket_data); - led.toggle(LED::BLUE); - - backend.send(packet); -} - -bool Telemetry::receive(TelemetryCommand* command, int wait_milliseconds) { - return backend.read(command, wait_milliseconds); -} - -void Telemetry::acknowledgeReceived() { - received_count ++; -} - /** * @brief creates the packet to send through the telemetry system * * @param data the data to serialize into a packet */ -TelemetryPacket Telemetry::makePacket(RocketData& data) { - +TelemetryPacket makePacket(RocketData& data, int received_count) { TelemetryPacket packet { }; - GPS gps = data.gps.getRecentUnsync(); - Voltage voltage = data.voltage.getRecentUnsync(); - Barometer barometer = data.barometer.getRecentUnsync(); + GPSData gps = data.gps.getRecentUnsync(); + VoltageData voltage = data.voltage.getRecentUnsync(); + BarometerData barometer = data.barometer.getRecentUnsync(); FSMState fsm = data.fsm_state.getRecentUnsync(); - Continuity continuity = data.continuity.getRecentUnsync(); + ContinuityData continuity = data.continuity.getRecentUnsync(); HighGData highg = data.high_g.getRecentUnsync(); PyroState pyro = data.pyro.getRecentUnsync(); - Orientation orientation = data.orientation.getRecentUnsync(); + OrientationData orientation = data.orientation.getRecentUnsync(); KalmanData kalman = data.kalman.getRecentUnsync(); packet.lat = gps.latitude; @@ -106,7 +77,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) { // Roll rate constexpr float max_roll_rate_hz = 10.0f; constexpr float max_kf_altitude = 40000.0f; - float roll_rate_hz = std::clamp(std::abs(orientation.angular_velocity.vx) / (2.0f*static_cast(PI)), 0.0f, max_roll_rate_hz); + float roll_rate_hz = std::clamp(std::abs(orientation.angular_velocity.vx) / (2.0f*static_cast(M_PI)), 0.0f, max_roll_rate_hz); float kf_px_clamped = std::clamp(kalman.position.px, 0.0f, max_kf_altitude); const float max_volts = 12; @@ -121,18 +92,9 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) { float kf_vx_clamped = std::clamp(kalman.velocity.vx, -2000.f, 2000.f); packet.kf_vx = (uint16_t) ((kf_vx_clamped + 2000) / 4000. * ((1 << 16) - 1)); - #ifdef IS_SUSTAINER +#ifdef IS_SUSTAINER packet.fsm_callsign_satcount |= (1 << 7); - #endif +#endif return packet; } - -/** - * @brief initializes the Telemetry system - * - * @return Error Code -*/ -ErrorCode __attribute__((warn_unused_result)) Telemetry::init() { - return backend.init(); -} diff --git a/MIDAS/src/telemetry.h b/MIDAS/src/telemetry.h index 2f6c2df4..2f0bbb28 100644 --- a/MIDAS/src/telemetry.h +++ b/MIDAS/src/telemetry.h @@ -1,36 +1,43 @@ #pragma once +#include "hal.h" #include "telemetry_packet.h" #include "rocket_state.h" -#include "errors.h" #include "led.h" -#if defined(SILSIM) -#include "silsim/emulated_telemetry.h" -#elif defined(HILSIM) -#include "hilsim/telemetry_backend.h" -#else -#include "hardware/telemetry_backend.h" -#endif +TelemetryPacket makePacket(RocketData& data, int received_count); /** * @class Telemetry * * @brief wraps the telemetry system to create and send a packet -*/ + */ +template class Telemetry { public: - Telemetry() = default; - explicit Telemetry(TelemetryBackend&& backend); + Telemetry(HwInterface& hw, LEDController& led) : hw(hw), led(led) { } - ErrorCode __attribute__((warn_unused_result)) init(); + void transmit(RocketData& rocket_data) { + static_assert(sizeof(TelemetryPacket) <= 0xFF, "The data type to send is too large"); // Max payload is 255 + + TelemetryPacket packet = makePacket(rocket_data, received_count); + led.toggle(LED::BLUE); + + hw.transmit(&packet); + } + + bool receive(TelemetryCommand* command, int wait_milliseconds) { + static_assert(sizeof(TelemetryCommand) <= 0xFF, "The data type to receive is too large"); // Max payload is 255 + return hw.receive(command, wait_milliseconds); + } + + void acknowledgeReceived() { + received_count++; + } - void transmit(RocketData& rocket_data, LEDController& led); - bool receive(TelemetryCommand* command, int wait_milliseconds); - void acknowledgeReceived(); private: - int received_count; - TelemetryPacket makePacket(RocketData& data); + int received_count = 0; - TelemetryBackend backend; + HwInterface& hw; + LEDController& led; };