/*
 * PID.h
 *
 *  Created on: Nov 10, 2014
 *      Author: ucart
 */

#ifndef PID_H_
#define PID_H_

#include "type_def.h"

// Yaw constants

// when using units of degrees
//#define YAW_ANGULAR_VELOCITY_KP 40.0f
//#define YAW_ANGULAR_VELOCITY_KI 0.0f
//#define YAW_ANGULAR_VELOCITY_KD 0.0f
//#define YAW_ANGLE_KP 2.6f
//#define YAW_ANGLE_KI 0.0f
//#define YAW_ANGLE_KD 0.0f

// when using units of radians
#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
#define YAW_ANGULAR_VELOCITY_KI 0.0f
#define YAW_ANGULAR_VELOCITY_KD 0.0f
#define YAW_ANGLE_KP 2.6f
#define YAW_ANGLE_KI 0.0f
#define YAW_ANGLE_KD 0.0f

// Roll constants
//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
//#define ROLL_ANGLE_KP 17.0f //9.0f
//#define ROLL_ANGLE_KI 0.0f
//#define ROLL_ANGLE_KD 0.3f // 0.2f
//#define YPOS_KP 0.0f
//#define YPOS_KI 0.0f
//#define YPOS_KD 0.0f

// when using units of radians
#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
#define ROLL_ANGLE_KP 15.0f
#define ROLL_ANGLE_KI 0.0f
#define ROLL_ANGLE_KD 0.2f
#define YPOS_KP 0.015f
#define YPOS_KI 0.005f
#define YPOS_KD 0.03f


//Pitch constants

// when using units of degrees
//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
//#define PITCH_ANGLE_KP 17.0f // 7.2f
//#define PITCH_ANGLE_KI 0.0f
//#define PITCH_ANGLE_KD 0.3f //0.3f
//#define XPOS_KP 40.0f
//#define XPOS_KI 0.0f
//#define XPOS_KD 10.0f//0.015f

// when using units of radians
#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
#define PITCH_ANGLE_KP 15.0f
#define PITCH_ANGLE_KI 0.0f
#define PITCH_ANGLE_KD 0.2f
#define XPOS_KP -0.015f
#define XPOS_KI -0.005f
#define XPOS_KD -0.03f


//Throttle constants
#define ALT_ZPOS_KP 9804.0f
#define ALT_ZPOS_KI 817.0f
#define ALT_ZPOS_KD 7353.0f

// Computes control error and correction
PID_values pid_computation(PID_t *pid);

#endif /* PID_H_ */