Added support for multiple processes on capable microcontrollers

main/config/birdbrain.h

@@ -10,13 +10,17 @@ constexpr uint32_t TICK_TIME_ASCENDING = 10000;
 constexpr uint32_t TICK_TIME_DESCENDING = 50000;
 constexpr uint32_t TICK_TIME_POST_FLIGHT = 1000000;
 
-constexpr uint32_t GPS_WAIT_TIME = 900; //number of seconds to wait for GPS to acquire signal before moving on
+constexpr uint32_t GPS_WAIT_TIME = 300; //number of seconds to wait for GPS to acquire signal before moving on
 
-constexpr uint32_t RING_QUEUE_LENGTH = 1000;//each 100 elements is 1 seconds of data at 100hz
+//constexpr uint32_t RING_QUEUE_LENGTH = 1000;//each 100 elements is 1 seconds of data at 100hz
+
+constexpr uint32_t RING_QUEUE_LENGTH_IMU = 2000;
+constexpr uint32_t RING_QUEUE_LENGTH_ALTIMETER = 1000;
+constexpr uint32_t RING_QUEUE_LENGTH_GPS = 200;
 
 // pin definitions
 constexpr int BMP_CS = 7; //SPI chipSelect of bmp altimeter
-constexpr int BUILTIN_SDCARD = 9; // SPI chipSelect of SD card
+constexpr int BUILTIN_SDCARD = 2; // SPI chipSelect of SD card
 constexpr int RADIO_CS = 8; //SPI chipSelect of radio module
 constexpr int IMU_CS = 6; //SPI chipSelect of IMU
 constexpr int SPI0_SCK = 2; //SPI clock pin
@@ -28,8 +32,10 @@ constexpr int SPI1_MISO = 12;
 constexpr int BUZZER_PIN = 26;
 constexpr int PYRO0_PIN = 14;
 constexpr int PYRO1_PIN = 15;
+constexpr int E220_M0_PIN = 3;
+constexpr int E220_M1_PIN = 4;
 
 constexpr int TONE_HZ = 2500; //frequency of buzzer tone
 
 #define GPSSerial Serial1 //Hardware Serial Location of GPS
-#define XBeeSerial Serial //Hardware Serial Location of XBee
+#define XBeeSerial Serial2 //Hardware Serial Location of XBee

main/controller/calculations/attitude.cpp

@@ -0,0 +1,111 @@
+//angles are in radians
+//rates are magnitudes of rad/s
+//assuming roll == 0, pitchRate will be equivalent to y, and yawRate will be equivalent to x
+
+#include "attitude.h"
+
+#include "./utils/Ewma.h" 
+
+//returns calculated pitchRate or yawRate
+/**
+ * @brief Calculate Pitch or Yaw rate given raw inputs and current roll orientation
+ * 
+ * @note Imagine your rocket has a sensor on it that detects X and Y pitch. If the Vehicle rolls 90 degrees, these sensors will become swapped. 
+ *    If the vehicle rolls 180 degrees, the sensors will be reading the exact opposite of reality. This function normalizes the input using simple trig.
+ * 
+ * @param roll current roll orientation
+ * @param parallel raw rate in one direction (X)
+ * @param perpendicular raw rate in perpendicular direction (Y)
+ * @return double actual rate in cardinal X direction
+ */
+double Attitude::getRate(const double roll, const double parallel, const double perpendicular)
+{   
+    //if getting rateX, parallel should be x
+    return parallel * cos(roll) + perpendicular * sin(roll);
+}
+
+/**
+ * @brief Filter raw data through an EWMA to compute "Zero Rate Offset" for each axis
+ * 
+ * @param rawGyro One sample of the gyroscope
+ */
+void Attitude::calibrateGyro(const Directional& rawGyro)
+{
+    static Directional lastSavedOffsets;
+    static int16_t tickCounter;
+    static Ewma xFilter(.002);
+    static Ewma yFilter(.002);
+    static Ewma zFilter(.002);
+
+    xFilter.filter(rawGyro.x);
+    yFilter.filter(rawGyro.y);
+    zFilter.filter(rawGyro.z);
+
+    tickCounter = (tickCounter + 1) % 500;
+    //every 500 ticks (2-5 seconds), we save the last state and transfer the previous state to the go queue
+    if(tickCounter == 0){
+        m_zeroRateOffsets = lastSavedOffsets;
+        lastSavedOffsets = Directional(xFilter.getOutput(), yFilter.getOutput(), zFilter.getOutput());
+    }
+}
+
+/**
+ * @brief normalize raw gyro data using zero rate offsets
+ * 
+ * @param[in, out] gyro raw gyro sample
+ */
+void Attitude::getCalibratedGyro(Directional& gyro)
+{
+    gyro.x = gyro.x - m_zeroRateOffsets.x;
+    gyro.y = gyro.y - m_zeroRateOffsets.y;
+    gyro.z = gyro.z - m_zeroRateOffsets.z;
+}
+
+/**
+ * @brief Calculate current attitude (orientation)
+ * 
+ * @note In order to minimize error, we should assume that attitude is 0,0,0 at launch. However, 
+ *  we don't actually know exactly when launch occurs. We can assume with almost 100% 
+ *  certainty that launch is detected less than 500 ticks (2-5 seconds) after it has actually occured, so 
+ *  as a substitute, we can start recording attitude changes 500 ticks before detected 
+ *  launch. In order to achieve this, we can record changes in 500 tick increments (chunks) 
+ *  prior to launch. Once launch is detected we will 'lock in' those chunks and continue 
+ *  recording changes forever.
+ * 
+ * @param gyro raw gyro sample
+ * @param hasLaunched has the rocket launched
+ */
+void Attitude::updateAttitude(Directional gyro, const bool hasLaunched, uint32_t sampleTime)
+{
+    static Directional oldChanges; //5-10 seconds ago
+    static Directional lastChanges; //0-5 seconds ago
+    static uint16_t tickCounter = 0;
+
+    getCalibratedGyro(gyro);
+    const uint32_t timeDiff = sampleTime - m_lastSampleTime;
+    m_lastSampleTime = sampleTime;
+    const double timeDiffSeconds = timeDiff / 1000000;
+
+    tickCounter = (tickCounter + 1) % 500;
+
+    lastChanges.z += gyro.z * timeDiffSeconds;
+    m_attitude.z = oldChanges.z + lastChanges.z; //z must be set first so we can calculate x and y
+    lastChanges.x += getRate(m_attitude.z/* -gyro.z/2 */, gyro.x, gyro.y) * timeDiffSeconds; //TODO: test whether or not -gyro.z/2 makes a difference
+    lastChanges.y += getRate(m_attitude.z/* -gyro.z/2 */, gyro.y, gyro.x) * timeDiffSeconds;
+    m_attitude.x = oldChanges.x + lastChanges.x;
+    m_attitude.y = oldChanges.y + lastChanges.y;
+
+    // every 5 seconds we delete all data that's more than 5 seconds old (prior to launch)
+    // this minimizes accumalation of error before launch
+    if(tickCounter == 0 && !hasLaunched){
+        oldChanges = lastChanges;
+        lastChanges.x = 0;
+        lastChanges.y = 0;
+        lastChanges.z = 0;
+    }
+}
+
+Directional Attitude::getCurrentAttitude()
+{
+    return m_attitude;
+}

main/controller/calculations/attitude.h

@@ -0,0 +1,26 @@
+/*****************************************************************************
+ * This file is for all math used to determine attitude
+ * 
+ ****************************************************************************/
+
+//angles are in radians
+//rates are magnitudes of rad/s
+//assuming roll == 0, pitchRate will be equivalent to y, and yawRate will be equivalent to x
+
+#pragma once
+
+#include "./utils/datatypes.h"
+
+/// @brief this object tracks attitude/orientation as well as gyroscope calibration values
+class Attitude {
+public:
+  Attitude() = default;
+  void calibrateGyro(const Directional& rawGyro);
+  void updateAttitude(Directional gyro, const bool hasLaunched, const uint32_t sampleTime);
+  Directional getCurrentAttitude();
+private:
+  Directional m_attitude, m_zeroRateOffsets;
+  uint32_t m_lastSampleTime;
+  double getRate(const double roll, const double parallel, const double perpendicular);
+  void getCalibratedGyro(Directional& gyro);
+};

main/controller/controller.h

@@ -0,0 +1,55 @@
+/*****************************************************************************************
+ * 
+ * Interface for main functions to be used either in a single process or multiprocess
+ * 
+*****************************************************************************************/
+
+#pragma once
+
+#include "calculations/attitude.h"
+#include "sensors/imu/imu.h"
+#include "data/data.h"
+#include "utils/datatypes.h"
+
+
+class RocketController {
+    public:
+        RocketController();
+        ~RocketController() = default;
+
+        /// @brief setup sensors and other hardware
+        void setup();
+
+        /// @brief setup second core
+        void setup1();
+
+        /// @brief main loop for the main core
+        void loop();
+
+        /// @brief main loop for the second core
+        void loop1();
+    private:
+        /// @brief run the on pad phase
+        FlightPhase runOnPad();
+
+        /// @brief run the ascending phase
+        FlightPhase runAscending();
+
+        /// @brief run the descending phase
+        FlightPhase runDescending();
+
+        /// @brief run the post flight phase
+        FlightPhase runPostFlight();
+
+        /// @brief initialize pyro channels
+        void setupPyros();
+
+        volatile FlightPhase state;
+        uint32_t tick;
+
+        IMU imu;
+        SDCard sd;
+
+        // current attitude (orientation) of the rocket
+        Attitude attitude;
+};