69 lines
1.9 KiB
C
69 lines
1.9 KiB
C
#include "attitude.h"
|
|
#include "pid.h"
|
|
#include "imu.h"
|
|
#include "imu_processing.h"
|
|
#include <math.h>
|
|
|
|
#define RATE_LIM 300.0f
|
|
|
|
float angle_kp_pitch = 2.5f;
|
|
float angle_kp_roll = 2.5f;
|
|
float angle_kp_yaw = 2.0f;
|
|
|
|
pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f};
|
|
pid_t pid_roll = {.kp = 0.6f, .kd = 0.025f};
|
|
pid_t pid_yaw = {.kp = 0.6f};
|
|
|
|
void attitude_init(attitude_t* att)
|
|
{
|
|
att->gyro = (Vector3){0};
|
|
}
|
|
|
|
void attitude_controller_update(control_channels_t* control,
|
|
const rc_channels* rx,
|
|
const Quaternion* current_q,
|
|
const Vector3* gyro)
|
|
{
|
|
Quaternion q_target = rx_to_quaternion(rx);
|
|
|
|
Quaternion q_error = QuatGetError(current_q, &q_target, true);
|
|
|
|
Vector3 angle_error =
|
|
{
|
|
2.0f * q_error.x,
|
|
2.0f * q_error.y,
|
|
2.0f * q_error.z
|
|
};
|
|
|
|
Vector3 desired_rate =
|
|
{
|
|
angle_error.x * angle_kp_pitch,
|
|
angle_error.y * angle_kp_roll,
|
|
angle_error.z * angle_kp_yaw
|
|
};
|
|
|
|
desired_rate.x = constrain(desired_rate.x, -RATE_LIM, RATE_LIM);
|
|
desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM);
|
|
desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM);
|
|
|
|
control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT);
|
|
control->roll = pid_update(&pid_roll, desired_rate.y - gyro->y, gyro->y, IMU_DT);
|
|
control->yaw = pid_update(&pid_yaw, desired_rate.z - gyro->z, gyro->z, IMU_DT);
|
|
}
|
|
|
|
Quaternion rx_to_quaternion(const rc_channels* rx)
|
|
{
|
|
float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
|
|
float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
|
|
float yaw = 0;
|
|
|
|
Vector3 pry = {pitch, roll, yaw};
|
|
|
|
return QuatCreateFromEuler(&pry);
|
|
}
|
|
|
|
float constrain(float x, float min, float max)
|
|
{
|
|
if (x < min) x = min; else if (x > max) x = max;
|
|
return x;
|
|
} |