add new angle algorithm, start adding logger

src/Makefile

@@ -3,7 +3,7 @@ USER_LIB_PATH = ./libraries
 OBJDIR        = ./.build-$(BOARD_TAG)
 
 BOARD_TAG    = teensy31
-ARDUINO_LIBS = i2c_t3 MedianFilter EEPROM
+ARDUINO_LIBS = i2c_t3 MedianFilter EEPROM SPI SD
 
 F_CPU=120000000
 

src/flight_controller.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#define ANGLE_SAFETY_STOP true
+#define ANGLE_SAFETY_STOP false
 #define SAFE_ANGLE 60.0
 
 typedef enum {

src/imu.cpp

@@ -1,6 +1,7 @@
 #include "imu.h"
 #include "MedianFilter.h"
 #include "utils.h"
+#include "quaternion_filters.h"
 
 static axis_float_t gyro_angles;
 static axis_float_t accel_angles;
@@ -82,13 +83,19 @@ static void combine() {
   float dt = (float) (micros() - combination_update_timer) / 1000000.0;
   combination_update_timer = micros();
 
-  accel_angles.x = atan2(accel_filtered.y, accel_filtered.z) * RAD_TO_DEG;
-  accel_angles.y = atan2(-1 * accel_filtered.x,
-    sqrt(accel_filtered.y * accel_filtered.y + accel_filtered.z * accel_filtered.z)
-  ) * RAD_TO_DEG;
-
-  angles.x = GYRO_PART * (angles.x + (rates.x * dt)) + ACC_PART * accel_angles.x;
-  angles.y = GYRO_PART * (angles.y + (rates.y * dt)) + ACC_PART * accel_angles.y;
+  // accel_angles.x = atan2(accel_filtered.y, accel_filtered.z) * RAD_TO_DEG;
+  // accel_angles.y = atan2(-1 * accel_filtered.x,
+  //   sqrt(accel_filtered.y * accel_filtered.y + accel_filtered.z * accel_filtered.z)
+  // ) * RAD_TO_DEG;
+  //
+  // angles.x = GYRO_PART * (angles.x + (rates.x * dt)) + ACC_PART * accel_angles.x;
+  // angles.y = GYRO_PART * (angles.y + (rates.y * dt)) + ACC_PART * accel_angles.y;
+
+  madgwick_quaternion_update(
+    &angles, dt,
+    accel_filtered.x, accel_filtered.y, accel_filtered.z,
+    rates.x * DEG_TO_RAD, rates.y * DEG_TO_RAD, rates.z * DEG_TO_RAD
+  );
 }
 
 void imu_benchmark() {

src/logger.cpp

@@ -0,0 +1,53 @@
+#include "logger.h"
+#include <Arduino.h>
+#include <SD.h>
+#include <SPI.h>
+
+// #include "imu.h"
+// #include "utils.h"
+// #include "pids.h"
+// #include "remote_control.h"
+// #include "motors.h"
+// #include "flight_controller.h"
+// #include "serial_commands.h"
+// #include "battery_monitor.h"
+
+bool logger_active = false;
+
+void logger_init() {
+  Serial.print("Initializing SD card...");
+
+  if (!SD.begin(LOGGER_SPI2_CHIP_SELECT)) {
+    Serial.println("Card failed, or not present");
+    return;
+  }
+
+  logger_active = true;
+  Serial.println("card initialized.");
+}
+
+void logger_log() {
+  if (!logger_active) return;
+
+  // make a string for assembling the data to log:
+  String dataString = "";
+
+  // open the file. note that only one file can be open at a time,
+  // so you have to close this one before opening another.
+  File dataFile = SD.open("datalog.txt", FILE_WRITE);
+  //  dataFile.close();
+  //
+  //  dataFile = SD.open("datalog.txt", FILE_WRITE);
+
+  // if the file is available, write to it:
+  if (dataFile) {
+    dataFile.println(dataString);
+    dataFile.close();
+    // print to the serial port too:
+    Serial.println(dataString);
+  }
+  // if the file isn't open, pop up an error:
+  else {
+    Serial.println("error opening datalog.txt");
+  }
+}

src/logger.h

@@ -0,0 +1,6 @@
+#pragma once
+
+#define LOGGER_SPI2_CHIP_SELECT 14
+
+void logger_init();
+void logger_log();

src/main.cpp

@@ -11,7 +11,7 @@
 #include "config.h"
 #include "watchdog.h"
 #include "battery_monitor.h"
-
+#include "logger.h"
 #include "utils.h"
 
 void setup() {
@@ -24,6 +24,7 @@ void setup() {
   fc_init();
   debugger_leds_init();
   battery_monitor_init();
+  // logger_init();
   watchdog_enable();
 }
 

src/quaternion_filters.cpp

@@ -0,0 +1,109 @@
+#include "quaternion_filters.h"
+#include "utils.h"
+#include "Arduino.h"
+
+float GyroMeasError = PI * (40.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
+
+float GyroMeasDrift = PI * (2.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
+
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+
+void calculate_euler_angles(axis_float_t *angles) {
+  float euler_pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+  float euler_roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+
+  angles->x = euler_roll * RAD_TO_DEG;
+  angles->y = euler_pitch * RAD_TO_DEG;
+}
+
+// from:  https://github.com/kriswiner/MPU-6050
+void madgwick_quaternion_update(axis_float_t *angles, float deltat, float ax, float ay, float az, float gx, float gy, float gz) {
+  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];         // short name local variable for readability
+  float norm;                                               // vector norm
+  float f1, f2, f3;                                         // objetive funcyion elements
+  float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
+  float qDot1, qDot2, qDot3, qDot4;
+  float hatDot1, hatDot2, hatDot3, hatDot4;
+  float gerrx, gerry, gerrz, gbiasx = 0.0, gbiasy = 0.0, gbiasz = 0.0;        // gyro bias error
+
+  // Auxiliary variables to avoid repeated arithmetic
+  float _halfq1 = 0.5f * q1;
+  float _halfq2 = 0.5f * q2;
+  float _halfq3 = 0.5f * q3;
+  float _halfq4 = 0.5f * q4;
+  float _2q1 = 2.0f * q1;
+  float _2q2 = 2.0f * q2;
+  float _2q3 = 2.0f * q3;
+  float _2q4 = 2.0f * q4;
+  // float _2q1q3 = 2.0f * q1 * q3;
+  // float _2q3q4 = 2.0f * q3 * q4;
+
+  // Normalise accelerometer measurement
+  norm = sqrt(ax * ax + ay * ay + az * az);
+  if (norm == 0.0f) return; // handle NaN
+  norm = 1.0f/norm;
+  ax *= norm;
+  ay *= norm;
+  az *= norm;
+
+  // Compute the objective function and Jacobian
+  f1 = _2q2 * q4 - _2q1 * q3 - ax;
+  f2 = _2q1 * q2 + _2q3 * q4 - ay;
+  f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
+  J_11or24 = _2q3;
+  J_12or23 = _2q4;
+  J_13or22 = _2q1;
+  J_14or21 = _2q2;
+  J_32 = 2.0f * J_14or21;
+  J_33 = 2.0f * J_11or24;
+
+  // Compute the gradient (matrix multiplication)
+  hatDot1 = J_14or21 * f2 - J_11or24 * f1;
+  hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
+  hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
+  hatDot4 = J_14or21 * f1 + J_11or24 * f2;
+
+  // Normalize the gradient
+  norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
+  hatDot1 /= norm;
+  hatDot2 /= norm;
+  hatDot3 /= norm;
+  hatDot4 /= norm;
+
+  // Compute estimated gyroscope biases
+  gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
+  gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
+  gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
+
+  // Compute and remove gyroscope biases
+  gbiasx += gerrx * deltat * zeta;
+  gbiasy += gerry * deltat * zeta;
+  gbiasz += gerrz * deltat * zeta;
+  gx -= gbiasx;
+  gy -= gbiasy;
+  gz -= gbiasz;
+
+  // Compute the quaternion derivative
+  qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
+  qDot2 =  _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
+  qDot3 =  _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
+  qDot4 =  _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;
+
+  // Compute then integrate estimated quaternion derivative
+  q1 += (qDot1 -(beta * hatDot1)) * deltat;
+  q2 += (qDot2 -(beta * hatDot2)) * deltat;
+  q3 += (qDot3 -(beta * hatDot3)) * deltat;
+  q4 += (qDot4 -(beta * hatDot4)) * deltat;
+
+  // Normalize the quaternion
+  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+  norm = 1.0f/norm;
+  q[0] = q1 * norm;
+  q[1] = q2 * norm;
+  q[2] = q3 * norm;
+  q[3] = q4 * norm;
+
+  calculate_euler_angles(angles);
+}

src/quaternion_filters.h

@@ -0,0 +1,8 @@
+#include "types.h"
+
+void madgwick_quaternion_update(
+  axis_float_t *angles,
+  float deltat,
+  float ax, float ay, float az,
+  float gx, float gy, float gz
+);