#pragma once

#include "fix16.h"

void ahrs_start();
void ahrs_stop();

void get_calibrated_mag(fix16_t *fx, fix16_t *fy, fix16_t *fz, fix16_t *norm);
void get_calibrated_acc(fix16_t *fx, fix16_t *fy, fix16_t *fz, fix16_t *norm);


// Values are in degrees
volatile extern fix16_t ahrs_heading;
volatile extern fix16_t ahrs_roll;
volatile extern fix16_t ahrs_pitch;
volatile extern fix16_t ahrs_inclination;

volatile extern unsigned ahrs_start_time;
volatile extern unsigned ahrs_updates;

// Sensor calibration data
extern int32_t AHRS_MAG_BIAS_X;
extern int32_t AHRS_MAG_BIAS_Y;
extern int32_t AHRS_MAG_BIAS_Z;
extern int32_t AHRS_MAG_SCALE_X;
extern int32_t AHRS_MAG_SCALE_Y;
extern int32_t AHRS_MAG_SCALE_Z;

extern int32_t AHRS_ACC_BIAS_X;
extern int32_t AHRS_ACC_BIAS_Y;
extern int32_t AHRS_ACC_BIAS_Z;
extern int32_t AHRS_ACC_SCALE_X;
extern int32_t AHRS_ACC_SCALE_Y;
extern int32_t AHRS_ACC_SCALE_Z;

// Extra bias introduced by the servo position
extern fix16_t AHRS_MAG_SERVO_BIAS_X_A;
extern fix16_t AHRS_MAG_SERVO_BIAS_X_B;
extern fix16_t AHRS_MAG_SERVO_BIAS_X_C;
extern fix16_t AHRS_MAG_SERVO_BIAS_Y_A;
extern fix16_t AHRS_MAG_SERVO_BIAS_Y_B;
extern fix16_t AHRS_MAG_SERVO_BIAS_Y_C;
extern fix16_t AHRS_MAG_SERVO_BIAS_Z_A;
extern fix16_t AHRS_MAG_SERVO_BIAS_Z_B;
extern fix16_t AHRS_MAG_SERVO_BIAS_Z_C;