void
init_pids()
{
	// create limits to how much dampening we'll allow
	// this creates symmetry with the P gain value preventing oscillations

	max_stabilize_dampener 	= g.pid_stabilize_roll.kP() * 2500;	// = 0.6 * 2500 = 1500 or 15°
	max_yaw_dampener		= g.pid_yaw.kP() * 6000;				// = .5 * 6000  = 3000
}


void
control_nav_mixer()
{
	// control +- 45° is mixed with the navigation request by the Autopilot
	// output is in degrees = target pitch and roll of copter
	g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
	g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
}

void
fbw_nav_mixer()
{
	// control +- 45° is mixed with the navigation request by the Autopilot
	// output is in degrees = target pitch and roll of copter
	g.rc_1.servo_out = nav_roll;
	g.rc_2.servo_out = nav_pitch;
}

void
output_stabilize_roll()
{
	float error, rate;
	int dampener;

	error 		= g.rc_1.servo_out - dcm.roll_sensor;

	// limit the error we're feeding to the PID
	error 		= constrain(error,  -2500, 2500);

	// write out angles back to servo out - this will be converted to PWM by RC_Channel
	g.rc_1.servo_out 	= g.pid_stabilize_roll.get_pid(error,  	delta_ms_fast_loop, 1.0);

	// We adjust the output by the rate of rotation:
	// Rate control through bias corrected gyro rates
	// omega is the raw gyro reading

	// Limit dampening to be equal to propotional term for symmetry
	rate				= degrees(omega.x) * 100.0; 													// 6rad = 34377
	dampener 			= (rate * g.stabilize_dampener);										// 34377 * .175 = 6000
	g.rc_1.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
}

void
output_stabilize_pitch()
{
	float error, rate;
	int dampener;

	error 	= g.rc_2.servo_out - dcm.pitch_sensor;

	// limit the error we're feeding to the PID
	error 	= constrain(error, -2500, 2500);

	// write out angles back to servo out - this will be converted to PWM by RC_Channel
	g.rc_2.servo_out 	= g.pid_stabilize_pitch.get_pid(error, 	delta_ms_fast_loop, 1.0);

	// We adjust the output by the rate of rotation:
	// Rate control through bias corrected gyro rates
	// omega is the raw gyro reading

	// Limit dampening to be equal to propotional term for symmetry
	rate				= degrees(omega.y) * 100.0; 													// 6rad = 34377
	dampener 			= (rate * g.stabilize_dampener);										// 34377 * .175 = 6000
	g.rc_2.servo_out	-= constrain(dampener,  -max_stabilize_dampener, max_stabilize_dampener);	// limit to 1500 based on kP
}

void
clear_yaw_control()
{
	//Serial.print("Clear ");
	rate_yaw_flag  	= false;			// exit rate_yaw_flag
	nav_yaw 		= dcm.yaw_sensor;	// save our Yaw
	yaw_error 		= 0;
}

void
output_yaw_with_hold(boolean hold)
{
	if(hold){
		// look to see if we have exited rate control properly - ie stopped turning
		if(rate_yaw_flag){
			// we are still in motion from rate control
			if(fabs(omega.y) < .15){
				clear_yaw_control();
				hold 			= true;			// just to be explicit
			}else{
				// return to rate control until we slow down.
				hold = false;
			}
		}

	}else{
		// rate control

		// this indicates we are under rate control, when we enter Yaw Hold and
		// return to 0° per second, we exit rate control and hold the current Yaw
		rate_yaw_flag 	= true;
		yaw_error 		= 0;
	}

	if(hold){
		// try and hold the current nav_yaw setting
		yaw_error		= nav_yaw - dcm.yaw_sensor; 									// +- 60°
		yaw_error 		= wrap_180(yaw_error);

		// limit the error we're feeding to the PID
		yaw_error		= constrain(yaw_error,   -6000, 6000);						// limit error to 60 degees

		// Apply PID and save the new angle back to RC_Channel
		g.rc_4.servo_out 	= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .5 * 6000 = 3000

		// We adjust the output by the rate of rotation
		long rate		= degrees(omega.z) * 100.0; 									// 3rad = 17188 , 6rad = 34377
		int dampener 	= ((float)rate * g.hold_yaw_dampener);						// 18000 * .17 = 3000

		// Limit dampening to be equal to propotional term for symmetry
		g.rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000

	}else{
		// rate control
		long rate		= degrees(omega.z) * 100; 									// 3rad = 17188 , 6rad = 34377
		rate			= constrain(rate, -36000, 36000);							// limit to something fun!
		long error		= ((long)g.rc_4.control_in * 6) - rate;						// control is += 6000 * 6 = 36000
																					// -error = CCW, 	+error = CW
		g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
		g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -2400, 2400);					// limit to 2400

	}
}

void
output_rate_roll()
{
	// rate control
	long rate		= degrees(omega.x) * 100; 									// 3rad = 17188 , 6rad = 34377
	rate			= constrain(rate, -36000, 36000);							// limit to something fun!
	long error		= ((long)g.rc_1.control_in * 8) - rate;						// control is += 4500 * 8 = 36000

	g.rc_1.servo_out 	= g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
	g.rc_1.servo_out 	= constrain(g.rc_1.servo_out, -2400, 2400);					// limit to 2400
}

void
output_rate_pitch()
{
	// rate control
	long rate		= degrees(omega.y) * 100; 									// 3rad = 17188 , 6rad = 34377
	rate			= constrain(rate, -36000, 36000);							// limit to something fun!
	long error		= ((long)g.rc_2.control_in * 8) - rate;						// control is += 4500 * 8 = 36000

	g.rc_2.servo_out 	= g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); 	// .075 * 36000 = 2700
	g.rc_2.servo_out 	= constrain(g.rc_2.servo_out, -2400, 2400);					// limit to 2400
}

// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void
reset_I(void)
{
	g.pid_nav_lat.reset_I();
	g.pid_nav_lon.reset_I();
	g.pid_baro_throttle.reset_I();
	g.pid_sonar_throttle.reset_I();
}




/*************************************************************
throttle control
****************************************************************/

// user input:
// -----------
void output_manual_throttle()
{
	g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
}

// Autopilot
// ---------
void output_auto_throttle()
{
	g.rc_3.servo_out 	= (float)nav_throttle * angle_boost();
	// make sure we never send a 0 throttle that will cut the motors
	g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
}

void calc_nav_throttle()
{
	// limit error
	long error = constrain(altitude_error, -400, 400);
	
	if(altitude_sensor == BARO) {
		float t = g.pid_baro_throttle.kP();
		
		if(error > 0){ 					// go up
			g.pid_baro_throttle.kP(t);
		}else{							// go down
			g.pid_baro_throttle.kP(t/4.0);
		}
		
		// limit output of throttle control
		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
		
		g.pid_baro_throttle.kP(t);
		
	} else {
		// SONAR
		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
	
		// limit output of throttle control
		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
	}
	
	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
	nav_throttle_old = nav_throttle;
}

float angle_boost()
{
	float temp = cos_pitch_x * cos_roll_x;
	temp = 2.0 - constrain(temp, .7, 1.0);
	return temp;
}


/*************************************************************
yaw control
****************************************************************/

void output_manual_yaw()
{
	if(g.rc_3.control_in == 0){
		clear_yaw_control();
	} else {
		// Yaw control
		if(g.rc_4.control_in == 0){
			//clear_yaw_control();
			output_yaw_with_hold(true); // hold yaw
		}else{
			output_yaw_with_hold(false); // rate control yaw
		}
	}
}

void auto_yaw()
{
	output_yaw_with_hold(true); // hold yaw
}