Merged changes from main

.vscode/settings.json

@@ -0,0 +1,9 @@
+{
+    "files.associations": {
+        "*.tcc": "cpp",
+        "unordered_map": "cpp",
+        "numeric": "cpp",
+        "ostream": "cpp",
+        "cstdint": "cpp"
+    }
+}

config.json

@@ -0,0 +1,40 @@
+{
+    "BottleID": 1,
+    "SSID": "UBC_UAS_Parachute_1",
+    "Password": "UBCUAS2023",
+    "AcquireRate": 57,
+    "GRAVITY": 9.809000015,
+    "ACC_X_STD": 0.3,
+    "ACC_Y_STD": 0.3,
+    "ACC_Z_STD": 0.05,
+    "BARO_ALT_STD": 1.466,
+    "GPS_POS_STD": 2.5,
+    "PID": {
+      "KP": 1,
+      "KI": 0,
+      "KD": 0
+    },
+    "Print_Enable": true,
+    "BufferSize": 512,
+    "OutputData": {
+      "Barometer": {
+        "Altitude": true,
+        "Pressure": true
+      },
+      "IMU": {
+        "LinearAccel": true,
+        "Orientation_Quaternion": true,
+        "EulerAngles": true
+      },
+      "KalmanFilter": {
+        "Velocity": true,
+        "Position": true
+      },
+      "GPS": {
+        "Coordinates": true,
+        "Altitude": true,
+        "Satellites": true,
+        "Lock": true
+      }
+    }
+  }

python/measureGpsRate.py

@@ -0,0 +1,33 @@
+import serial
+import time
+
+# Replace '/dev/ttyUSB0' with your GPS device's serial port
+serial_port = 'COM5'
+# Replace 9600 with your GPS device's baud rate
+baud_rate = 921600
+# Duration over which to measure the data rate (in seconds)
+measure_duration = 60
+
+def measure_data_rate():
+    try:
+        with serial.Serial(serial_port, baud_rate, timeout=1) as ser:
+            start_time = time.time()
+            data_received = 0  # in bytes
+
+            while time.time() - start_time < measure_duration:
+                data = ser.readline()
+                data_received += len(data)
+
+                # Optional: Print the received data
+                # print(data.decode('utf-8'), end='')
+
+            # Calculate and print the data rate
+            data_rate = data_received / measure_duration
+            print(f"Data rate: {data_rate} bytes/second")
+            print(f"Total Data received: {data_received}")
+
+    except serial.SerialException as e:
+        print(f"Error opening serial port {serial_port}: {e}")
+
+if __name__ == "__main__":
+    measure_data_rate()

sensorDemo/SteeringTest/PID_peanat.cpp

@@ -0,0 +1,201 @@
+/* 
+ * PID library made for robots!
+ * Extra: custom initial output, deadband limits
+ * Date created: Aug. 16, 2022
+ */
+
+#include "Arduino.h"
+#include <math.h>
+#include "PID_peanat.h"
+
+const double SECS_IN_MS = 0.001;
+
+
+/* 
+ * Initialize PID object.
+ *
+ * @param user_kp: proportional coefficient, should be >= 0
+ * @param user_ki: integral coefficient, should be >= 0
+ * @param user_kd: derivative coefficient, should be >= 0
+ * 
+ * @returns PID object
+ */
+PID::PID(double user_kp, double user_ki, double user_kd) {
+    kp = user_kp;
+    ki = user_ki;
+    kd = user_kd;
+    init_output = 0.0;
+    reverse = false;
+    sample_time = 100;
+    min_output = 0.0;
+    max_output = 255.0;
+    deadband = 0.0;
+
+    // Reverse coefficients if in reverse mode
+    if (reverse == true) {
+        kp *= -1.0;
+        ki *= -1.0;
+        kd *= -1.0;
+    }
+
+    // Initiate other fields
+    first_run = true;
+    cumulative_error = 0.0;
+}
+
+/*
+ * Updates PID coefficients.
+ * 
+ * @param user_kp: proportional coefficient, should be >= 0
+ * @param user_ki: integral coefficient, should be >= 0
+ * @param user_kd: derivative coefficient, should be >= 0
+ * 
+ * @returns none
+ */
+void PID::updateCoeffs(double user_kp, double user_ki, double user_kd) {
+    kp = user_kp;
+    ki = user_ki;
+    kd = user_kd;
+}
+
+/*
+ * Set initial output; must be within or on output bounds.
+ *
+ * @param user_init_output: desired initial output, for when we want the output to start at a specific value
+ * 
+ * @returns none
+ */
+void PID::setInitOutput(double user_init_output) {
+    init_output = user_init_output;
+}
+
+/*
+ * Toggle reverse mode on or off.
+ *
+ * @param user_reverse: true to turn reverse mode on, false to turn off
+ * 
+ * @returns none
+ */
+void PID::setReverse(bool user_reverse) {
+   if (reverse != user_reverse) {
+        reverse = user_reverse;
+        kp *= -1.0;
+        ki *= -1.0;
+        kd *= -1.0;
+   }
+}
+
+/*
+ * Set sample time (recommended < 100).
+ * 
+ * @param user_sample_time: sample time in ms
+ * 
+ * @returns none
+ */
+void PID::setSampleTime(unsigned long user_sample_time) {
+    sample_time = user_sample_time;
+}
+
+/*
+ * Set output bounds; useful if something like a servo has input limits.
+ * Make sure that the initial output is on or within these bounds!
+ *
+ * @param user_min_output: minimum output value (inclusive)
+ * @param user_max_output: maximum output value (inclusive)
+ * 
+ * @returns none
+ */
+void PID::setOutputBounds(double user_min_output, double user_max_output) {
+    min_output = user_min_output;
+    max_output = user_max_output;
+}
+
+/*
+ * Set deadband range (difference between previous and current calculated output, in which the 
+ * previous output is returned) if we need to prevent mechanical wear. Deadband must be less
+ * than max output - min output.
+ * 
+ * @param user_deadband: deadband range, in units of output
+ * 
+ * @returns none
+ */
+void PID::setDeadband(double user_deadband) {
+    deadband = user_deadband;
+}
+
+/*
+ * Computes PID output based on standard PID algorithm; implements deadband functionality and
+ * measures to prevent integral windup. The first run will output the initial output value.
+ * This loop can run for a maximum of 49 days before unsigned long overflows.
+ * 
+ * @param user_setpoint: desired value of something (e.g. servo position)
+ * @param user_input: the acutal value
+ * 
+ * @returns output to be fed back into the controller
+ */
+double PID::compute(double user_setpoint, double user_input) {
+    // If first run, initialize variables
+    if (first_run == true) {
+        setpoint = user_setpoint;
+        input = user_input;
+
+        prev_time = millis();
+        prev_error = setpoint - input;
+        prev_output = init_output;
+
+        output = init_output;
+        first_run = false;
+    }
+
+    // Subsequent runs
+    else {
+        current_time = millis();
+        unsigned long time_interval = current_time - prev_time;
+
+        if (time_interval >= sample_time) {
+            setpoint = user_setpoint;
+            input = user_input;
+            double error = setpoint - input;
+
+            // Proportional
+            output = init_output + kp * error;
+            // Integral
+            cumulative_error += ki * error * ((double)time_interval * SECS_IN_MS);
+            cumulative_error = min(max(cumulative_error, min_output), max_output); // Limit cumulative error to prevent windup
+            output += cumulative_error;
+            // Derivative
+            output += kd * (prev_error - error) / ((double)time_interval * SECS_IN_MS);
+
+            // Limit output
+            output = min(max(output, min_output), max_output);
+
+            // Don't change output if within deadband
+            if (fabs(output - prev_output) < deadband) {
+                output = prev_output;
+            }
+
+            // For the next run
+            prev_time = current_time;
+            prev_error = error;
+            prev_output = output;
+        }
+    }
+
+    return output;
+}
+
+double PID::getOutput() {
+    return output;
+}
+
+double PID::getkp() {
+    return kp;
+}
+
+double PID::getki() {
+    return ki;
+}
+
+double PID::getkd() {
+    return kd;
+}

sensorDemo/SteeringTest/PID_peanat.h

@@ -0,0 +1,63 @@
+#ifndef PID_PEANAT_H
+#define PID_PEANAT_H
+
+/*
+ * This is a PID class to be used in PID control loops.
+ */
+
+class PID {
+
+    public:
+
+    // Constructor
+    PID(double user_kp, double user_ki, double user_kd);
+
+    // Change settings
+    void updateCoeffs(double user_kp, double user_ki, double user_kd);
+    void setInitOutput(double user_init_output);
+    void setReverse(bool user_reverse);
+    void setSampleTime(unsigned long user_sample_time);
+    void setOutputBounds(double user_min_output, double user_max_output);
+    void setDeadband(double user_deadband);
+
+    // Calculate PID output
+    double compute(double user_setpoint, double user_input);
+
+    // Get private variables
+    double getOutput();
+    double getkp();
+    double getki();
+    double getkd();
+
+
+    private:
+
+    double setpoint;                        // Target value
+
+    double input;                           // What the value really is
+
+    double output;                          // Calculated output
+    double init_output;                     // Output value to start from
+    double prev_output;                     // Previous output
+
+    double kp;                              // Proportional coefficient
+    double ki;                              // Integral coefficient
+    double kd;                              // Derivative coefficient
+
+    double prev_error;                      // Previous error
+    double cumulative_error;                // Cumulative error for calculating integral output
+
+    unsigned long current_time;             // Current time in milliseconds
+    unsigned long prev_time;                // Time at which previous input was taken
+    unsigned long sample_time;              // Input sample time
+
+    double min_output;                      // Max output
+    double max_output;                      // Min output
+
+    double deadband;                        // The range in which we don't change the output (to prevent mechanical wear)
+
+    bool reverse;                           // Reverse mode (if true, output * -1)
+    bool first_run;                         // True if it is the very first call to the `compute` function
+};
+
+#endif

sensorDemo/SteeringTest/SteeringTest.ino

@@ -0,0 +1,40 @@
+// #include "advancedSteering.h"
+#include <Arduino.h>
+#include "encoder_steering.h"
+
+double desired_heading = 0;
+double current_heading = 0;
+
+
+
+void setup() {
+  Serial.begin(921600); // Start serial communication at 921600 baud rate
+  steering_setup(100.0, 50.0, 0, 0); // Initialize steering
+}
+
+void loop() {
+  // Check if we have incoming data
+  if (Serial.available()) {
+    String inputString = Serial.readStringUntil('\n');  // Read the incoming data until newline
+    inputString.trim();  // Trim whitespace and newline characters
+
+    // Parse the string to extract desired and current headings
+    double incomingDesiredHeading, incomingCurrentHeading;
+    if (sscanf(inputString.c_str(), "%lf,%lf", &incomingDesiredHeading, &incomingCurrentHeading) == 2) {
+      // Successfully parsed two doubles
+      // Update the global variables with the new data
+      desired_heading = incomingDesiredHeading * 0.01745; // Convert to radians
+      current_heading = incomingCurrentHeading * 0.01745; // Convert to radians
+      Serial.printf("Desired heading: %lf, Current heading: %lf\n", desired_heading, current_heading);
+    } else {
+      Serial.println("Invalid format or incomplete data");
+    }
+    // create the data packet
+    AngleData data = {desired_heading, current_heading, 0};
+    // send the data packet to the queue
+    sendSteeringData(data);
+  }
+
+  Serial.printf("Count: %d %d\n", count_1, count_2);
+  delay(100);
+}