Initial commit

Source/Control/Inc/attitude.h

@@ -0,0 +1,35 @@
+#ifndef ATTITUDE_H
+#define ATTITUDE_H
+
+#include "imu_processing.h"
+#include "radio_receiver.h"
+#include "pid.h"
+
+#define CF_ALPHA        0.99f
+
+typedef struct
+{
+  float roll;   // deg
+  float pitch;  // deg
+  float yaw_rate;    // deg/s
+} attitude_t;
+
+void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);
+void attitude_update(attitude_t* attitude, imu_scaled_t* imu);
+void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu);
+void attitude_pid_update(control_channels_t* control, 
+                         const rc_channels* rx, 
+                         const attitude_t* att, 
+                         const imu_scaled_t* imu);
+void TIM6_DAC_IRQHandler();
+
+float accel_roll_deg(const imu_scaled_t* imu);
+float accel_pitch_deg(const imu_scaled_t* imu);
+
+void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu);
+void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu);
+void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu);
+
+
+
+#endif

Source/Control/Inc/biquad_filter.h

@@ -0,0 +1,20 @@
+#ifndef BIQUAD_FILTER_H
+#define BIQUAD_FILTER_H
+
+#include <math.h>
+
+#define PI      3.14159265359f
+
+typedef struct
+{
+  float b0, b1, b2;
+  float a1, a2;
+
+  float x1, x2;
+  float y1, y2;
+} biquad_t;
+
+void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate);
+float biquad_apply(biquad_t* f, float input);
+
+#endif

Source/Control/Inc/pid.h

@@ -0,0 +1,26 @@
+#ifndef PID_H
+#define PID_H
+
+#include "stm32g431xx.h"
+
+typedef struct
+{
+  float kp;
+  float ki;
+  float kd;
+  
+  float integral;
+  float prev_error;
+} pid_t;
+
+typedef struct
+{
+  float roll;
+  float pitch;
+  float yaw;
+} control_channels_t;
+
+float pid_update(pid_t* pid, float error, float gyro_rate, float dt);
+
+
+#endif