#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