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