/*************************
输出给电机的PWM计算
*************************/
void MOTOR_CAL(void)
{
Angle_Time_dt = Get_Angle_PIDtime();
pid_pitch = PID_Calculate(&PID_Pitch_Struct, Pitch, Motor_Angle_X, Angle_Time_dt);
pid_roll = PID_Calculate(&PID_Roll_Struct, Roll, Motor_Angle_Y, Angle_Time_dt);
// printf("\n PID_ROLL=%f, PID_PITCH=%f, PID_YAW=%f \n\r", pid_pitch, pid_roll, pid_z);
// printf("\n PID_Pitch=%f, PID_Roll=%f \n\r", pid_pitch, pid_roll);
//+模式
MOTOR1 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Roll_PlusDirec(Motor_Angle_Y)+pid_roll); //Y轴负向电机
MOTOR2 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Roll_MinusDirec(Motor_Angle_Y)-pid_roll); //Y轴正向电机
MOTOR3 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Pitch_PlusDirec(Motor_Angle_X)+pid_pitch); //X轴负向电机
MOTOR4 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Pitch_MinusDirec(Motor_Angle_X)-pid_pitch); //X轴正向电机
// printf("\n pid_pitch_plusdirec_initval=%f, Motor4=%d \n\r", pid_pitch_plusdirec_initval, MOTOR4);
if(Motor_Thr<=1050)//防止未加油门时，由于四轴倾斜或旋转导致电机转动
{
MOTOR1=1000;
MOTOR2=1000;
MOTOR3=1000;
MOTOR4=1000;
}
}
/* 角度环 */
void CtrlAttiAng(void)
{
Angle_Time_dt = Get_Angle_PIDtime();
PID_Calculate(&PID_Pitch_Struct, Pitch, Motor_Angle_X, Angle_Time_dt);
PID_Calculate(&PID_Roll_Struct, Roll, Motor_Angle_Y, Angle_Time_dt);
// printf("\n t=%f, PID_Pitch=%f, PID_Roll=%f \n\r", Angle_Time_dt, PID_Pitch_Struct.Ouput_deltaUk, PID_Roll_Struct.Ouput_deltaUk);
}
//run in 200Hz or 400Hz loop
void CtrlAttiRate(void)
{
Rate_Time_dt = Get_Rate_PIDtime();
pid_pitch = PID_Calculate(&PID_Pitch_Rate_Struct, gy, PID_Pitch_Struct.Ouput_deltaUk, Rate_Time_dt); //绕x轴为gy,Pitch
pid_roll = PID_Calculate(&PID_Roll_Rate_Struct, gx, PID_Roll_Struct.Ouput_deltaUk, Rate_Time_dt); //绕Y轴为gx，Roll
// printf("\n t=%f, PID_Pitch=%f, PID_Roll=%f \n\r", Rate_Time_dt, pid_pitch, pid_roll);
MOTOR1 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Roll_PlusDirec(Motor_Angle_Y)+pid_roll); //Y轴负向电机
MOTOR2 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Roll_MinusDirec(Motor_Angle_Y)-pid_roll); //Y轴正向电机
MOTOR3 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Pitch_PlusDirec(Motor_Angle_X)+pid_pitch); //X轴负向电机
MOTOR4 = (uint16_t)Limit_PWMOUT(SpeedInitVal_Pitch_MinusDirec(Motor_Angle_X)-pid_pitch); //X轴正向电机
}
void TIM6_IRQHandler(void)
{
static float CurentValue_MOTOR[3];
static portBASE_TYPE xStatus;
static Moment_Typedef Moment;
static float Moments[3];
static long PWM_MOTOR[3];
static uint16_t Count = 0;
if(TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
{
while(FIR_CollectData(SEN_MOTOR1, TSVN_ACS712_Read(ACS_1)) != DONE);
CurentValue_MOTOR[MOTOR1] = AMES_Filter(SEN_MOTOR1) - ACS1_CALIB;
while(FIR_CollectData(SEN_MOTOR2 ,TSVN_ACS712_Read(ACS_2)) != DONE);
CurentValue_MOTOR[MOTOR2] = AMES_Filter(SEN_MOTOR2) - ACS2_CALIB;
while(FIR_CollectData(SEN_MOTOR3,TSVN_ACS712_Read(ACS_3)) != DONE);
CurentValue_MOTOR[MOTOR3] = AMES_Filter(SEN_MOTOR3) - ACS3_CALIB;
if (Count++>= 300)
{
printf("%0.5f\n", CurentValue_MOTOR[MOTOR1]);
Count = 0;
}
PWM_MOTOR[MOTOR1] = PID_Calculate(MOTOR1, 550.0, CurentValue_MOTOR[MOTOR1]);
if (PWM_MOTOR[MOTOR1] < 0)
DIR_Change(MOTOR1, RESERVE);
else
DIR_Change(MOTOR1, FORWARD);
TSVN_PWM_TIM5_Set_Duty(abs(PWM_MOTOR[MOTOR1]), MOTOR1_PWM);
TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
// PWM_MOTOR[MOTOR2] = PID_Calculate(MOTOR2, 300.0, CurentValue_MOTOR[MOTOR2]);
// if (PWM_MOTOR[MOTOR2] < 0)
// DIR_Change(MOTOR2, RESERVE);
// else
// DIR_Change(MOTOR2, FORWARD);
// TSVN_PWM_TIM5_Set_Duty(abs(PWM_MOTOR[MOTOR2]), MOTOR2_PWM);
//
// PWM_MOTOR[MOTOR3] = PID_Calculate(MOTOR3, 300.0, CurentValue_MOTOR[MOTOR3]);
//
// if (PWM_MOTOR[MOTOR3] < 0)
// DIR_Change(MOTOR3, RESERVE);
// else
// DIR_Change(MOTOR3, FORWARD);
// TSVN_PWM_TIM5_Set_Duty(abs(PWM_MOTOR[MOTOR3]), MOTOR3_PWM);
//
if (uxQueueMessagesWaitingFromISR(Moment_Queue) != NULL)
{
xStatus = xQueueReceiveFromISR(Moment_Queue, &Moment, 0);
if (xStatus == pdPASS)
{
Moments[0] = Moment.Mx;
Moments[1] = Moment.My;
Moments[2] = Moment.Mz;
}
}
}
}
//函数名：Controler()
//输入：无
//输出: 无
//描述：飞机控制函数主体，被定时器调用
//作者：马骏
//备注：没考上研，心情不好
void Controler(void)
{
static char Counter_Cnt=0;
Counter_Cnt++;
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
DMP_getYawPitchRoll(); //读取 姿态角
/*******************向上位机发送姿态信息，如果要在PC上位机看实时姿态，需要把这里解注释****************/
/*******************PC姿态显示，跟串口debug不能共存****************/
Send_AtitudeToPC();
if(Counter_Cnt==5)
{
Counter_Cnt=0;
Nrf_Irq(); //从2.4G接收控制目标参数
//ReceiveDataFormUART();//从蓝牙透传模块接收控制目标参数，和2.4G接收控制只能选其一
PID_Calculate(); //=2时控制一次,频率500HZ
}
}
//函数名：Controler()
//输入：无
//输出: 无
//描述：飞机控制函数主体，被定时器调用
//作者：马骏
//备注：没考上研，心情不好
void Controler(void)
{
static char Counter_Cnt=0;
Counter_Cnt++;
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
DMP_getYawPitchRoll(); //读取 姿态角
/*******************向上位机发送姿态信息，如果要在PC上位机看实时姿态,宏开关控制***************/
#ifndef Debug
Send_AtitudeToPC();
#else
#endif
if(Counter_Cnt==5)
{
Counter_Cnt=0;
Nrf_Irq(); //从2.4G接收控制目标参数
//ReceiveDataFormUART();//从蓝牙透传模块接收控制目标参数，和2.4G接收控制只能选其一
PID_Calculate(); //=2时控制一次,频率200HZ
}
}
