/**
* \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], ¶meter.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();
}
/**
* \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], ¶meter.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();
}
