60 lines
1.7 KiB
C
60 lines
1.7 KiB
C
#ifndef ATTITUDE_H
|
|
#define ATTITUDE_H
|
|
|
|
#include "imu_processing.h"
|
|
#include "radio_receiver.h"
|
|
#include "pid.h"
|
|
#include "quaternion.h"
|
|
#include "vector.h"
|
|
|
|
#define CF_ALPHA 0.99f
|
|
#define GYRO_BIAS_LIM 0.5f
|
|
#define RATE_LIM 300.0f
|
|
|
|
/*typedef struct
|
|
{
|
|
float roll; // deg
|
|
float pitch; // deg
|
|
float yaw_rate; // deg/s
|
|
} attitude_t;*/
|
|
|
|
typedef struct
|
|
{
|
|
Quaternion orientation; // current orientation
|
|
Vector3 gyro; // current angular velocity
|
|
} attitude_t;
|
|
|
|
|
|
void attitude_init(attitude_t* att);
|
|
void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu);
|
|
void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
|
|
|
|
void TIM6_DAC_IRQHandler();
|
|
void attitude_update(attitude_t* attitude, imu_scaled_t* imu);
|
|
void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
|
|
|
|
Vector3 gravity_from_quat(const Quaternion* q);
|
|
Quaternion rx_to_quaternion(const rc_channels* rx);
|
|
|
|
float constrain(float x, float min, float max);
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
void complementary_filter_update(attitude_t* att, const 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);*/
|
|
|
|
|
|
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 |