void Motor_status(MOTOR_T status)
{
if(status == 5)
{
Motor_Control(0,0);
}
else if (status > 5)
{
Motor_Control(status*10,0);
}
else if (status < 5)
{
Motor_Control(0,(10-status)*10);
}
}
/*==========================================
函数名：CarAngleControl()
作用 ：车体角度控制，内环
备注 ：无
===========================================*/
void CarAngleControl(void)
{
int16s_t nP, nD;
nP = g_nCarAngle-g_CarAngleSet;
nD = g_nCarGyroVal;
nP = (int16s_t)(-nP * (UP_KP));
nD = (int16s_t)(-nD * (UP_KD));
nSpeed = nP + nD;
if(nSpeed > MOTOR_SPEED_SET_MAX)
{
nSpeed = MOTOR_SPEED_SET_MAX;
}
else if(nSpeed < MOTOR_SPEED_SET_MIN)
{
nSpeed = MOTOR_SPEED_SET_MIN;
}
nLeft = (int16s_t)nSpeed;
nRight = (int16s_t)nSpeed;
g_nLeftMotorOut = -nLeft;
g_nRightMotorOut = -nRight;
Motor_Control();
}
void Motor_status(uint8_t status)
{
u16 temp = (MOTOR_ARR+1) / MOTOR_MAX;
if(status > MOTOR_MAX)
status = MOTOR_MAX;
if(status < MOTOR_MAX1)
status = MOTOR_MAX1;
if(status == 5)
{
Motor_Control(0,0);
}
else if (status > 5)
{
Motor_Control(status*10,0);
}
else if (status < 5)
{
Motor_Control(0,(10-status)*10);
}
// switch (status)
// {
// case Motor_stop:
// GPIO_ResetBits(GPIOB,GPIO_Pin_8);
// GPIO_ResetBits(GPIOB,GPIO_Pin_9);
// break;
// case Motor_Forward:
// GPIO_SetBits(GPIOB,GPIO_Pin_8);
// GPIO_ResetBits(GPIOB,GPIO_Pin_9);
// break;
// case Motor_Reverse:
// GPIO_SetBits(GPIOB,GPIO_Pin_9);
// GPIO_ResetBits(GPIOB,GPIO_Pin_8);
// break;
// default :
// break;
// }
}
/* 40 sec idle time pass ... trun off moto */
void vTimerSystemIdle( xTimerHandle pxTimer )
{
pwm_flag = 0;
Pitch_desire = 0; //Desire angle of Pitch
Roll_desire = 0; //Desire angle of Roll
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
qprintf(xQueueUARTSend, "10 sec trun off motor\n\r");
}
void system_init(void)
{
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
PWM_Capture_Config();
Sensor_Config();
nRF24L01_Config();
#if configSD_BOARD
SDIO_Config();
#endif
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +4.0f;
PID_Pitch.Ki = 0;//0.002f;
PID_Pitch.Kd = +1.5f;
PID_Roll.Kp = +4.0f;
PID_Roll.Ki = 0;//0.002f;
PID_Roll.Kd = 1.5f;
PID_Yaw.Kp = +5.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +15.0f;
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
#if configFLIGHT_CONTROL_BOARD
/* nRF Check */
while ( nRF_Check()== ERROR );
/* Sensor Init */
while(Sensor_Init() == ERROR);
#endif
Delay_10ms(10);
/* Lock */
LED_R = 0;
LED_G = 1;
LED_B = 1;
sys_status = SYSTEM_INITIALIZED;
}
void PIT_ISR(void)
{
pit_ctr++;
dir_ctr++;
spd_per++;
//Speed_Control();
switch (pit_ctr) {
case 1:
Angle_Read();
break;
case 2:
Angle_Merge();
balance_pwm = Balance_PID(angle_balance, gyro_balance);
Motor_Control();
// Motor_Out(3000, 3000);
break;
case 3:
spd_ctr++;
if (spd_ctr > 20) {
//speed_pwm = Speed_PID(speed_l, speed_r);
spd_ctr = 0;
spd_per = 0;
}
break;
case 4:
dir_ctr++;
//Dir_VS();
//Scope();
if (dir_ctr > 4) {
//Dir_Control();
dir_ctr = 0;
dir_per = 0;
}
break;
case 5:
Speed_Read();
pit_ctr = 0;
break;
default:
printf("d");
break;
}
//Spd_Control();
//Dir_Contorl();
Scope();
}
void system_init(void)
{
LED_Config();
Serial_Config(Serial_Baudrate);
Motor_Config();
PWM_Capture_Config();
//IMU Config
Sensor_Config();
nRF24L01_Config();
//SD Config
if ((SD_status = SD_Init()) != SD_OK)
system.status = SYSTEM_ERROR_SD;
PID_Init(&PID_Pitch, 4.0, 0.0, 1.5);
PID_Init(&PID_Roll, 4.0, 0.0, 1.5);
PID_Init(&PID_Yaw, 5.0, 0.0, 15.0);
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (nRF_Check() == ERROR);
/* Sensor Init */
while (Sensor_Init() == ERROR);
Delay_10ms(10);
/* Lock */
SetLED(LED_R, DISABLE);
SetLED(LED_G, ENABLE);
SetLED(LED_B, ENABLE);
//Check if no error
if (system.status != SYSTEM_ERROR_SD)
system.status = SYSTEM_INITIALIZED;
}
/*=====================================================================================================*/
int main(void)
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
test_printf();
/* Throttle Config */
if (KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while (KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if (Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if (SysTick_Config(SystemCoreClock / SampleRateFreg)) { // SampleRateFreg = 500 Hz
while (1);
}
/* Wait Correction */
while (SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while (!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
printf("Roll%d,Pitch%d,Yaw%d,CH1 %u(%d),CH2 %u(%d),CH3 %u(%d),CH4 %u(%d),CH5 %u()\r\n",
(int)AngE.Roll, (int)AngE.Pitch, (int)AngE.Yaw,
PWM1_InputCaptureValue, global_rc_roll,
PWM2_InputCaptureValue, global_rc_pitch,
PWM3_InputCaptureValue, global_rc_thr,
PWM4_InputCaptureValue, global_rc_yaw,
PWM5_InputCaptureValue);
}
LED_B = 1;
/* Final State Machine */
while (1) {
LED_G = ~LED_G;
switch (FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while (Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if (Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0] + 20, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
/*=====================================================================================================*/
int main( void )
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
/* Throttle Config */
if(KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while(KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while(Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if(Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if(SysTick_Config(420000)) { // 168MHz / 420000 = 400Hz = 2.5ms
while(1);
}
/* Wait Correction */
while(SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while(!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
}
LED_B = 1;
/* Final State Machine */
while(1) {
LED_G = ~LED_G;
switch(FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while(Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if(Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
void flightControl_task()
{
//Waiting for system finish initialize
while (sys_status == SYSTEM_UNINITIALIZED);
sys_status = SYSTEM_FLIGHT_CONTROL;
while (1) {
GPIO_ToggleBits(GPIOC, GPIO_Pin_7);
uint8_t IMU_Buf[20] = {0};
int16_t Final_M1 = 0;
int16_t Final_M2 = 0;
int16_t Final_M3 = 0;
int16_t Final_M4 = 0;
int16_t Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;
int16_t Exp_Thr = 0, Exp_Pitch = 0, Exp_Roll = 0, Exp_Yaw = 0;
uint8_t safety = 0;
static uint8_t BaroCnt = 0;
/* 500Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
MPU9150_Read(IMU_Buf);
/* 100Hz, Read Barometer */
BaroCnt++;
if (BaroCnt == SampleRateFreg / 100) {
MS5611_Read(&Baro, MS5611_D1_OSR_4096);
BaroCnt = 0;
}
Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
Temp.T = (s16)((IMU_Buf[6] << 8) | IMU_Buf[7]);
Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
Mag.X = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
Mag.Y = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
Mag.Z = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);
/* Offset */
Acc.X -= Acc.OffsetX;
Acc.Y -= Acc.OffsetY;
Acc.Z -= Acc.OffsetZ;
Gyr.X -= Gyr.OffsetX;
Gyr.Y -= Gyr.OffsetY;
Gyr.Z -= Gyr.OffsetZ;
Mag.X *= Mag.AdjustX;
Mag.Y *= Mag.AdjustY;
Mag.Z *= Mag.AdjustZ;
if (SensorMode == Mode_Algorithm) {
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);
/* To Physical */
Acc.TrueX = Acc.X * MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y * MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z * MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X * MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y * MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z * MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB
/* Get Attitude Angle */
AHRS_Update();
global_var[TRUE_ROLL].param = AngE.Roll;
global_var[TRUE_PITCH].param = AngE.Pitch;
global_var[TRUE_YAW].param = AngE.Yaw;
/*Get RC Control*/
Update_RC_Control(&Exp_Roll, &Exp_Pitch, &Exp_Yaw, &Exp_Thr, &safety);
global_var[RC_EXP_THR].param = Exp_Thr;
global_var[RC_EXP_ROLL].param = Exp_Roll;
global_var[RC_EXP_PITCH].param = Exp_Pitch;
global_var[RC_EXP_YAW].param = Exp_Yaw;
/* Get ZeroErr */
PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch);
PID_Roll.ZeroErr = (float)((s16)Exp_Roll);
PID_Yaw.ZeroErr = (float)((s16)Exp_Yaw) + 180.0f;
/* PID */
Roll = (s16)PID_AHRS_Cal(&PID_Roll, AngE.Roll, Gyr.TrueX);
Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
// Yaw = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw, Gyr.TrueZ);
Yaw = (s16)(PID_Yaw.Kd * Gyr.TrueZ) + 3 * (s16)Exp_Yaw;
Thr = (s16)Exp_Thr;
global_var[PID_ROLL].param = Roll;
global_var[PID_PITCH].param = Pitch;
global_var[PID_YAW].param = Yaw;
/* Motor Ctrl */
Final_M1 = Thr + Pitch - Roll - Yaw;
Final_M2 = Thr + Pitch + Roll + Yaw;
Final_M3 = Thr - Pitch + Roll - Yaw;
Final_M4 = Thr - Pitch - Roll + Yaw;
global_var[MOTOR1].param = Final_M1;
global_var[MOTOR2].param = Final_M2;
global_var[MOTOR3].param = Final_M3;
global_var[MOTOR4].param = Final_M4;
Bound(Final_M1, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
Bound(Final_M2, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
Bound(Final_M3, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
Bound(Final_M4, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
if (safety == 1) {
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
} else {
Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);
}
vTaskDelay(2);
}
}
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
u8 IMU_Buf[20] = {0};
u16 Final_M1 = 0;
u16 Final_M2 = 0;
u16 Final_M3 = 0;
u16 Final_M4 = 0;
s16 Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;
float Ellipse[5] = {0};
static u8 BaroCnt = 0;
static s16 ACC_FIFO[3][256] = {0};
static s16 GYR_FIFO[3][256] = {0};
static s16 MAG_FIFO[3][256] = {0};
static s16 MagDataX[8] = {0};
static s16 MagDataY[8] = {0};
static u16 Correction_Time = 0;
/* Time Count */
SysTick_Cnt++;
if(SysTick_Cnt == SampleRateFreg) {
SysTick_Cnt = 0;
Time_Sec++;
if(Time_Sec == 60) { // 0~59
Time_Sec = 0;
Time_Min++;
if(Time_Sec == 60)
Time_Min = 0;
}
}
/* 400Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
MPU9150_Read(IMU_Buf);
/* 100Hz, Read Barometer */
BaroCnt++;
if(BaroCnt == 4) {
MS5611_Read(&Baro, MS5611_D1_OSR_4096);
BaroCnt = 0;
}
Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
Temp.T = (s16)((IMU_Buf[6] << 8) | IMU_Buf[7]);
Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
Mag.X = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
Mag.Y = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
Mag.Z = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);
/* Offset */
Acc.X -= Acc.OffsetX;
Acc.Y -= Acc.OffsetY;
Acc.Z -= Acc.OffsetZ;
Gyr.X -= Gyr.OffsetX;
Gyr.Y -= Gyr.OffsetY;
Gyr.Z -= Gyr.OffsetZ;
Mag.X *= Mag.AdjustX;
Mag.Y *= Mag.AdjustY;
Mag.Z *= Mag.AdjustZ;
#define MovegAveFIFO_Size 250
switch(SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
LED_R = !LED_R;
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_MagCorrect; // Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
LED_R = ~LED_R;
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Mode_CorrectMag **************************************/
#define MagCorrect_Ave 100
#define MagCorrect_Delay 600 // 2.5ms * 600 = 1.5s
case Mode_MagCorrect:
LED_R = !LED_R;
Correction_Time++;
switch((u16)(Correction_Time/MagCorrect_Delay)) {
case 0:
LED_B = 0;
MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 1:
LED_B = 1;
MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 2:
LED_B = 0;
MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 3:
LED_B = 1;
MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 4:
LED_B = 0;
MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 5:
LED_B = 1;
MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 6:
LED_B = 0;
MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 7:
LED_B = 1;
MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
default:
LED_B = 1;
EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
Mag.EllipseSita = Ellipse[0];
Mag.EllipseX0 = Ellipse[1];
Mag.EllipseY0 = Ellipse[2];
Mag.EllipseA = Ellipse[3];
Mag.EllipseB = Ellipse[4];
Correction_Time = 0;
SensorMode = Mode_Quaternion;
break;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
LED_R = !LED_R;
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
Ellipse[3] = ( Mag.X*arm_cos_f32(Mag.EllipseSita)+Mag.Y*arm_sin_f32(Mag.EllipseSita))/Mag.EllipseB;
Ellipse[4] = (-Mag.X*arm_sin_f32(Mag.EllipseSita)+Mag.Y*arm_cos_f32(Mag.EllipseSita))/Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
/************************** Algorithm Mode ****************************************/
case Mode_Algorithm:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
/* Get Attitude Angle */
AHRS_Update();
// if(KEYL_U == 0) { PID_Roll.Kp += 0.001f; PID_Pitch.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Roll.Kp -= 0.001f; PID_Pitch.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Roll.Ki += 0.0001f; PID_Pitch.Ki += 0.0001f; }
// if(KEYL_D == 0) { PID_Roll.Ki -= 0.0001f; PID_Pitch.Ki -= 0.0001f; }
// if(KEYR_R == 0) { PID_Roll.Kd += 0.0001f; PID_Pitch.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Roll.Kd -= 0.0001f; PID_Pitch.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; PID_Pitch.SumErr = 0.0f; }
// if(KEYL_U == 0) { PID_Yaw.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Yaw.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Yaw.Ki += 0.001f; }
// if(KEYL_D == 0) { PID_Yaw.Ki -= 0.001f; }
// if(KEYR_R == 0) { PID_Yaw.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Yaw.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; }
/* Get ZeroErr */
PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch/4.5f);
PID_Roll.ZeroErr = (float)((s16)Exp_Roll/4.5f);
PID_Yaw.ZeroErr = (float)((s16)Exp_Yaw)+180.0f;
/* PID */
Roll = (s16)PID_AHRS_Cal(&PID_Roll, AngE.Roll, Gyr.TrueX);
Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
// Yaw = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw, Gyr.TrueZ);
Yaw = (s16)(PID_Yaw.Kd*Gyr.TrueZ);
Thr = (s16)Exp_Thr;
/* Motor Ctrl */
Final_M1 = PWM_M1 + Thr + Pitch + Roll + Yaw;
Final_M2 = PWM_M2 + Thr - Pitch + Roll - Yaw;
Final_M3 = PWM_M3 + Thr - Pitch - Roll + Yaw;
Final_M4 = PWM_M4 + Thr + Pitch - Roll - Yaw;
/* Check Connection */
#define NoSignal 1 // 1 sec
if(KEYR_L == 0)
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else if(((Time_Sec-RecvTime_Sec)>NoSignal) || ((Time_Sec-RecvTime_Sec)<-NoSignal))
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else
Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);
/* DeBug */
Tmp_PID_KP = PID_Yaw.Kp*1000;
Tmp_PID_KI = PID_Yaw.Ki*1000;
Tmp_PID_KD = PID_Yaw.Kd*1000;
Tmp_PID_Pitch = Yaw;
break;
}
}
/* sample rate and calculate rate (4ms,250HZ) */
void vTimerSample(xTimerHandle pxTimer)
{
LIS3DSH_Read(Buffer_Hx, LIS3DSH_OUT_X_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Hy, LIS3DSH_OUT_Y_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Lx, LIS3DSH_OUT_X_L_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Ly, LIS3DSH_OUT_Y_L_REG_ADDR, 1);
x_acc = (float)((int16_t)(Buffer_Hx[0] << 8 | Buffer_Lx[0]) - XOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
y_acc = (float)((int16_t)(Buffer_Hy[0] << 8 | Buffer_Ly[0]) - YOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
x_gyro = (float)((int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]) - GXOffset) * Sensitivity_250 / 1000;
y_gyro = (float)((int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]) - GYOffset) * Sensitivity_250 / 1000;
z_gyro = (float)((int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]) - GZOffset) * Sensitivity_250 / 1000;
angle_x = (0.99f) * (angle_x + y_gyro * 0.004f) - (0.01f) * (x_acc);
angle_y = (0.99f) * (angle_y + x_gyro * 0.004f) + (0.01f) * (y_acc);
argv.Pitch = angle_y; //pitch degree
argv.Roll = angle_x; //roll degree
argv.Pitch_v = x_gyro; //pitch velocity
argv.Roll_v = y_gyro; //Roll velocity
argv.Pitch_err = Pitch_desire - argv.Pitch;
argv.Roll_err = Roll_desire - argv.Roll;
ROLL = (int)(argv.RollP * argv.Roll_err - argv.RollD * argv.Roll_v);
PITCH = (int)(argv.PitchP * argv.Pitch_err - argv.PitchD * argv.Pitch_v);
YAW = (int)(argv.YawD * z_gyro);
if (PITCH > MAXNUM) {
PITCH = MAXNUM;
}else if (PITCH < MINNUM) {
PITCH = MINNUM;
}else{
PITCH = PITCH;
}
if (ROLL > MAXNUM) {
ROLL = MAXNUM;
}else if (ROLL < MINNUM) {
ROLL = MINNUM;
}else{
ROLL = ROLL;
}
if(argv.Roll > 35 || argv.Roll < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(argv.Pitch > 35 || argv.Pitch < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(pwm_flag == 0){
}else{
Motor1 = PWM_Motor1_tmp + PITCH - ROLL - YAW; //LD4
Motor2 = PWM_Motor2_tmp + PITCH + ROLL + YAW; //LD3
Motor3 = PWM_Motor3_tmp - PITCH + ROLL - YAW; //LD5
Motor4 = PWM_Motor4_tmp - PITCH - ROLL + YAW; //LD6
Motor_Control(Motor1, Motor2, Motor3, Motor4);
}
}
