/**
 *  \brief Interrupt-Service-Routine of the Control_Timer
 *
 *  \details Central Control Routine for Quadcopter\n
 *  This routine gets the momentary position and the desired position \n
 *  and calculates new output for motors and transmits it via DaisyChain to the \n
 *  Electric Speed Controller
 *
 */
void Control_Timer_ISR(void)
{
	float YPR[3];
	float powerD = 0.0f;/* throttle value of remote, range: 0 - 100*/
	float yawD_dot = 0.0f;/* yaw value of remote, range: -90 - +90*/
	float pitchD = 0.0f;/* pitchD value of remote, range: -30 - +30*/
	float rollD = 0.0f;/* rollD value of remote, range: -30 - +30*/
	static float x_pitch[CONTROL_ORDER];
	static float x_roll[CONTROL_ORDER];
	float u_yaw_dot = 0.0f;/* output of angle-rate controller for yaw*/
	float u_pitch = 0.0f;/* output of angle controller for pitch*/
	float u_roll = 0.0f;/* output of angle controller for roll*/
	float PWM_percent[4];/* scaled motor power from controller output, range: 0 - 100*/
	uint8_t height_control = 0;/* for enabling height-control, activated: 0xff, disabled: 0x00*/

	GetAngles(YPR);
	GetRCData(&powerD, &height_control, &yawD_dot, &pitchD, &rollD);
	//yaw control
	AngleRateController(&yawD_dot, &YPR[0], &parameter.P_yaw, &u_yaw_dot);
	//pitch control
	AngleController(&pitchD, &YPR[1], CONTROL_ORDER, polynoms.a_pitch, polynoms.b_pitch, x_pitch, &u_pitch);
	//roll control
	AngleController(&rollD, &YPR[2], CONTROL_ORDER, polynoms.a_roll, polynoms.b_roll, x_roll, &u_roll);
	//generate actuator values
	CreatePulseWidth(&u_roll, &u_pitch, &u_yaw_dot, &powerD, PWM_percent);

	if (powerD > 5.0f)
		SendDaisyPWMPercentages(PWM_percent);
	else
		SendDaisyStopCommand();

}
Example #2
0
void AttControl_TIMER_ISR(void)
{
	GetAngles(YPR,&yoffset);
	GetRCData(&powerD, &yawD_dot, &pitchD, &rollD);

	uint32_t Now = millis();
	float dt = ((Now - timePrev)/1000.0f);
	timePrev = Now;

	// filter
	float delta_yaw = YPR[0]-yaw;
	if (delta_yaw >= 180.0)
		delta_yaw-=360;
	if (delta_yaw <= -180.0)
		delta_yaw+=360;

	yaw+=0.3*delta_yaw;
	if (yaw >= 180.0)
		yaw-=360;
	if (yaw <= -180.0)
		yaw+=360;

	//derive
	delta_yaw=yaw-yaw_old;
	if (delta_yaw >= 180.0)
		delta_yaw-=360;
	if (delta_yaw <= -180.0)
		delta_yaw+=360;
	yaw_dot=delta_yaw/dt;

	yaw_old=yaw;

	//yaw control
	AngleRateController(&yawD_dot, &yaw_dot, &P_yaw, &u_yaw_dot);
	//AngleController(&yawD_dot, &yaw_dot, CONTROL_ORDER, a_yaw, b_yaw, x_yaw, &u_yaw_dot);
	//pitch control
	AngleController(&pitchD, &YPR[1], CONTROL_ORDER, a_pitch, b_pitch, x_pitch, &u_pitch);
	//roll control
	AngleController(&rollD, &YPR[2], CONTROL_ORDER, a_roll, b_roll, x_roll, &u_roll);

	counter_main++;
	newvalue=1;
}
/**
 *  \brief Interrupt-Service-Routine of the Control_Timer
 *
 *  \details Central Control Routine for Quadcopter\n
 *  This routine gets the momentary position and the desired position \n
 *  and calculates new output for motors and transmits it via DaisyChain to the \n
 *  Electric Speed Controller
 *
 */
void Control_Timer_ISR(void)
{
	static float x_pitch[CONTROL_ORDER];
	static float x_roll[CONTROL_ORDER];

	GetAngles(YPR);
	GetRCData(&powerD, &height_control, &yawD_dot, &pitchD, &rollD);
	//yaw control
	AngleRateController(&yawD_dot, &YPR[0], &parameter.P_yaw, &u_yaw_dot);
	//pitch control
	AngleController(&pitchD, &YPR[1], CONTROL_ORDER, polynoms.a_pitch, polynoms.b_pitch, x_pitch, &u_pitch);
	//roll control
	AngleController(&rollD, &YPR[2], CONTROL_ORDER, polynoms.a_roll, polynoms.b_roll, x_roll, &u_roll);
	//generate actuator values
	CreatePulseWidth(&u_roll, &u_pitch, &u_yaw_dot, &powerD, PWM_percent);

	if (powerD > 5.0f)
		SendDaisyPWMPercentages(PWM_percent);
	else
		SendDaisyStopCommand();

}