/**************************************************************************
* NAME: pid.h      		                                                  *
* AUTHOR: Johan Gärtner                                                   *
*
* PURPOSE: This file contains pid functions		                          *
* INFORMATION: pidUAV is the final pid controller which only takes        *
* 			   pidProfile_t as input. pidProfile_t will be updated and 	  *
* 			   contains all the information about the current state of    *
* 			   the pid.													  *
* GLOBAL VARIABLES:                                                       *
* Variable        Type          Description                               *
* --------        ----          -----------                               *
*
* **************************************************************************/
#ifndef FLIGHT_PID_H_
#define FLIGHT_PID_H_

#include<stdio.h>
#include<stdint.h>
#include "Flight/filter.h"
#include "drivers/accel_gyro.h"

#define XYZ_AXIS_COUNT 3					/*The maximum number of DOF that belongings to the PID*/

#define PID_ID_GYRO 0
#define PID_ID_ACCELEROMETER 1
#define PID_ID_COMPASS 2
#define PID_ID_BAROMETER 3
#define PID_COUNT 4
#define THROTTLE 0							/*Index terms to the PID*/

/*Enum of index to different profiles*/
typedef enum {
	ROLL = 0,
	PITCH,
	YAW
} RollPitchYaw;

/*Struct that belongs to a certain PID controller*/
typedef struct pidProfile_s {

	bool pidEnabled;
	uint8_t ID_profile;						/*ID of a certain PID, shall be referenced to a certain sensor*/

	uint8_t P[XYZ_AXIS_COUNT];				/*PID value*/
	uint8_t I[XYZ_AXIS_COUNT];				/*PID value*/
	uint8_t D[XYZ_AXIS_COUNT];				/*PID value*/

	uint16_t dterm_lpf;  					/*Cut of frequency for derivative (delta) term of the PID to the low pass filter, recommended settings 80 - 90 hz (same as clean flight)*/
	uint16_t pterm_yaw_lpf;					/*Cut of frequency for PTerm of the PID for yaw axis (low pass filter), recommended settings? (same as clean flight)*/
	int16_t yaw_p_limit;					/*The max and minimum output value of the PTerm of yaw axis*/

	int16_t pid_out_limit;					/*PID output limit*/

	uint8_t PIDweight[XYZ_AXIS_COUNT];		/*Used for TPA, should be 100 when default*/

	float PID_Out[XYZ_AXIS_COUNT];			/*PID out values from a certain controller*/

} pidProfile_t;

/*Array of all pid profiles of the system*/
extern pidProfile_t PidProfile[PID_COUNT];

/*  */
extern float accRollFineTune;
extern float accPitchFineTune;

extern accel_t accelProfile;

extern float throttleRate;
extern int HoverForce;/*Struct profile for input data from sensor*/

extern float Yaw;
extern float YawU;


/**************************************************************************
* BRIEF: Initializes PID profiles			 					          *
* INFORMATION: 															  *
**************************************************************************/
void pidInit();

/**************************************************************************
* BRIEF: Runs a certain PID Controller									  *
* INFORMATION:                       									  *
**************************************************************************/
void pidRun(uint8_t ID);

void readAcc(void);

void pidEproom(void);

#endif /* FLIGHT_PID_H_ */