/*
 * 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 radians
#define YAW_ANGULAR_VELOCITY_KP 200.0 * 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

// when using units of radians
#define ROLL_ANGULAR_VELOCITY_KP 100.0 * 46.0f
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
#define ROLL_ANGULAR_VELOCITY_KD 100.0 * 5.5f
#define ROLL_ANGLE_KP 15.0f
#define ROLL_ANGLE_KI 0.0f
#define ROLL_ANGLE_KD 0.2f
#define YPOS_KP 0.08f
#define YPOS_KI 0.01f
#define YPOS_KD 0.1f


//Pitch constants

// when using units of radians
#define PITCH_ANGULAR_VELOCITY_KP 100.0 * 46.0f
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
#define PITCH_ANGULAR_VELOCITY_KD 100.0 * 5.5f
#define PITCH_ANGLE_KP 15.0f
#define PITCH_ANGLE_KI 0.0f
#define PITCH_ANGLE_KD 0.2f
#define XPOS_KP 0.08f
#define XPOS_KI 0.01f
#define XPOS_KD 0.1f


//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_ */