//=============================================================================================
// MahonyAHRS.h
//=============================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// Date			Author			Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
//
//=============================================================================================
#ifndef _hal_mahonyAHRS_h
#define _hal_mahonyAHRS_h
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "sdk_common.h"
#include "SEGGER_RTT.h"
#include "usr_config.h"

typedef struct t_MahonyAHRS{
	float twoKi;                // 2 * integral gain (Ki)
	float twoKp;
	float q0, q1, q2, q3;        // quaternion of sensor frame relative to auxiliary frame
	float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
	float invSampleFreq;
	float roll, pitch, yaw;
	float q[4],accN[4];
	
}MahonyAHRS_t;

void Mahony_Init(MahonyAHRS_t *P,float sampleFrequency);
void Mahony_update(MahonyAHRS_t *P,float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len);
void Mahony_SetKp(MahonyAHRS_t *P,float twoKp);

//float Mahony_GetRoll(void);
//float Mahony_GetPitch(void);
//float Mahony_GetYaw(void);
//void Mahony_GetAccN(float* accn);

#endif