129 lines
3.3 KiB
C
129 lines
3.3 KiB
C
#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;
|
|
} |