Initial commit

Source/Control/Src/attitude.c

@@ -0,0 +1,129 @@
+#include "attitude.h"
+#include "pid.h"
+#include <math.h>
+
+volatile uint8_t imu_update_flag = 0;
+volatile uint8_t pid_update_flag = 0;
+
+float angle_kp_roll = 2.5f;
+float angle_kp_pitch = 2.5f;
+
+pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
+pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
+pid_t pid_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f};
+
+int16_t desired_roll = 0;
+int16_t desired_pitch = 0;
+
+float desired_roll_rate = 0.0f;
+float desired_pitch_rate = 0.0f;
+
+float roll_rate_error = 0.0f;
+float pitch_rate_error = 0.0f;
+float yaw_rate_error = 0.0f;
+
+float error_roll = 0.0f;
+float error_pitch = 0.0f;
+float error_yaw = 0.0f;
+
+void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
+{
+  static float roll_acc;
+  static float pitch_acc;
+  
+  roll_acc = accel_roll_deg(imu);
+  pitch_acc = accel_pitch_deg(imu);
+  
+  integrate_gyro_roll_deg(&att->roll, imu);
+  integrate_gyro_pitch_deg(&att->pitch, imu);
+  
+  att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
+  att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
+}
+
+void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
+{
+  if (imu_update_flag)
+  {
+    imu_update_flag = 0;
+    
+    imu_read_scaled(imu);
+    complementary_filter_update(attitude, imu);
+    yaw_rate_update(attitude, imu);
+  }
+}
+
+void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
+{
+  attitude->yaw_rate = imu->gz;
+}
+
+void attitude_pid_update(control_channels_t* control, 
+                         const rc_channels* rx, 
+                         const attitude_t* att,
+                         const imu_scaled_t* imu)
+{
+  if (pid_update_flag)
+  {
+    pid_update_flag = 0;
+    
+    desired_roll  = int_mapping(rx->rc_roll, -500, 500, -45, 45);
+    desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45);
+    
+    desired_roll_rate  = angle_kp_roll  * (desired_roll - att->roll);
+    desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch);
+    
+    if (desired_roll_rate > 200) desired_roll_rate = 200;
+    if (desired_roll_rate < -200) desired_roll_rate = -200;
+
+    if (desired_pitch_rate > 200)  desired_pitch_rate = 200;
+    if (desired_pitch_rate < -200) desired_pitch_rate = -200;
+    
+    roll_rate_error = desired_roll_rate - imu->gy;
+    pitch_rate_error = desired_pitch_rate - imu->gx;
+    yaw_rate_error = - imu->gz;
+    
+    control->roll  = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT);
+    control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
+    control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
+  }
+}
+
+void TIM6_DAC_IRQHandler()
+{
+  if (TIM6->SR & TIM_SR_UIF)
+  {
+    TIM6->SR &= ~TIM_SR_UIF;
+    imu_update_flag = 1;
+    pid_update_flag = 1;
+  }
+}
+
+float accel_roll_deg(const imu_scaled_t* imu) {
+  // right-left
+  return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
+}
+
+float accel_pitch_deg(const imu_scaled_t* imu)
+{
+  // forward-backward
+  return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI;
+}
+
+void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu)
+{
+  // right-left
+  *roll += imu->gy * IMU_DT;
+}
+
+void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu)
+{
+  // forward-backward
+  *pitch += imu->gx * IMU_DT;
+}
+
+void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu)
+{
+  // forward-backward
+  *yaw += imu->gz * IMU_DT;
+}

Source/Control/Src/biquad_filter.c

@@ -0,0 +1,43 @@
+#include "biquad_filter.h"
+
+void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate)
+{
+  float omega = 2.0f * PI * cutoff / sample_rate;
+  float sin_omega = sinf(omega);
+  float cos_omega = cosf(omega);
+
+  float alpha = sin_omega / sqrtf(2.0f);
+
+  float b0 = (1 - cos_omega) / 2;
+  float b1 = 1 - cos_omega;
+  float b2 = (1 - cos_omega) / 2;
+  float a0 = 1 + alpha;
+  float a1 = -2 * cos_omega;
+  float a2 = 1 - alpha;
+
+  f->b0 = b0 / a0;
+  f->b1 = b1 / a0;
+  f->b2 = b2 / a0;
+  f->a1 = a1 / a0;
+  f->a2 = a2 / a0;
+
+  f->x1 = f->x2 = 0;
+  f->y1 = f->y2 = 0;
+}
+
+float biquad_apply(biquad_t* f, float input)
+{
+  float output = f->b0 * input
+                 + f->b1 * f->x1
+                 + f->b2 * f->x2
+                 - f->a1 * f->y1
+                 - f->a2 * f->y2;
+
+  f->x2 = f->x1;
+  f->x1 = input;
+
+  f->y2 = f->y1;
+  f->y1 = output;
+
+  return output;
+}

Source/Control/Src/pid.c

@@ -0,0 +1,23 @@
+#include "pid.h"
+
+float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
+{
+  float p = pid->kp * error;
+  
+  pid->integral += error * dt;
+  if (pid->integral > 100) pid->integral = 100;
+  if (pid->integral < -100) pid->integral = -100;
+  
+  float i = pid->ki * pid->integral;
+  
+  float d = - pid->kd * gyro_rate;
+  
+  pid->prev_error = error;
+  
+  float spid = p + i + d;
+  
+  if (spid > 300) spid = 300;
+  if (spid < -300) spid = -300;
+  
+  return spid;
+}