Radzhab Bisultanov
0faafbf089
*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке. *Определение вращения работает корректно.
73 lines
2.0 KiB
C++
73 lines
2.0 KiB
C++
#include "Attitude.h"
|
|
#include "PID.h"
|
|
#include <math.h>
|
|
|
|
#define FREQUENCY 100.0f
|
|
#define PERIOD 1.0f / FREQUENCY
|
|
|
|
#define PI 3.14159265359f
|
|
#define DEG2RAD PI / 180.0f
|
|
#define RATE_LIM 300.0f
|
|
|
|
constexpr float angle_kp_pitch = 2.5f;
|
|
constexpr float angle_kp_roll = 2.5f;
|
|
constexpr 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){};
|
|
}
|
|
|
|
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 = current_q->GetError(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, PERIOD);
|
|
control->roll = pid_update(&pid_roll, desired_rate.Y - gyro->Y, gyro->Y, PERIOD);
|
|
control->yaw = pid_update(&pid_yaw, desired_rate.Z - gyro->Z, gyro->Z, PERIOD);
|
|
}
|
|
|
|
Quaternion rx_to_quaternion(const rc_channels* rx)
|
|
{
|
|
Quaternion q;
|
|
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};
|
|
q.CreatePitchRollYaw(pry);
|
|
return q;
|
|
}
|
|
|
|
float constrain(float x, float min, float max)
|
|
{
|
|
if (x < min) x = min; else if (x > max) x = max;
|
|
return x;
|
|
} |