diff --git a/project/p10-ANO/application/Attitude.c b/project/p10-ANO/application/Attitude.c index b4c8b37..f5fa8ef 100644 --- a/project/p10-ANO/application/Attitude.c +++ b/project/p10-ANO/application/Attitude.c @@ -10,26 +10,26 @@ float yaw, pitch, roll; //姿态角 void Attitude(void) { while (mpu_dmp_get_data(&pitch, &roll, &yaw)); - + static int gx_c, gy_c, gz_c; static short gx_w[cn], gy_w[cn], gz_w[cn]; static int wid; gx_c -= gx_w[wid]; gy_c -= gy_w[wid]; gz_c -= gz_w[wid]; - - gx_w[wid] = gyro[0]; - gy_w[wid] = gyro[1]; - gz_w[wid] = gyro[2]; + + gx_w[wid] = gyro[0]; + gy_w[wid] = gyro[1]; + gz_w[wid] = gyro[2]; gx_c += gx_w[wid]; gy_c += gy_w[wid]; gz_c += gz_w[wid]; - + if (++wid == cn) wid = 0; gyro[0] = gx_c/cn; gyro[1] = gy_c/cn; gyro[2] = gz_c/cn; - + // 函数内部把六轴原始数据放在全局变量 gyro 以及 accel 中 } diff --git a/project/p10-ANO/application/HMC5883.c b/project/p10-ANO/application/HMC5883.c index 65777ef..100e1a4 100644 --- a/project/p10-ANO/application/HMC5883.c +++ b/project/p10-ANO/application/HMC5883.c @@ -4,15 +4,16 @@ #include "math.h" #include "Attitude.h" - #define HMC5883_Addr 0x1E +#define HMC5883_Addr 0x1E - unsigned char BUF[8]; +unsigned char BUF[8]; short magRange0,magRange1,magRange2,magRange3,magRange4,magRange5; -short magX, magY, magZ; -float magX_off, magY_off, magZ_off; -float magX_cc, magY_cc, magZ_cc; - uint8_t HMC_Write_Byte(uint8_t reg, uint8_t data) +short magX, magY, magZ; +float magX_off, magY_off, magZ_off; +float magX_cc, magY_cc, magZ_cc; + +uint8_t HMC_Write_Byte(uint8_t reg, uint8_t data) { IIC_Start(); IIC_Send_Byte((HMC5883_Addr<<1)|0); @@ -33,8 +34,6 @@ float magX_cc, magY_cc, magZ_cc; return 0; } - - uint8_t HMC_Read_Byte(uint8_t reg) { uint8_t res; @@ -52,66 +51,62 @@ uint8_t HMC_Read_Byte(uint8_t reg) } - void InitHMC5883() - { - - HMC_Write_Byte(0x02,0x00); - HMC_Write_Byte(0x01,0xE0); +void InitHMC5883() +{ + + HMC_Write_Byte(0x02,0x00); + HMC_Write_Byte(0x01,0xE0); } - - void updateHMC5883() + +void updateHMC5883() { - - - HMC_Write_Byte(0x00,0x14); - HMC_Write_Byte(0x02,0x00); - // Delayms(10); - - BUF[1]=HMC_Read_Byte(0x03);//OUT_X_L_A - BUF[2]=HMC_Read_Byte(0x04);//OUT_X_H_A - BUF[3]=HMC_Read_Byte(0x07);//OUT_Y_L_A - BUF[4]=HMC_Read_Byte(0x08);//OUT_Y_H_A - BUF[5]=HMC_Read_Byte(0x05);//OUT_Z_L_A - BUF[6]=HMC_Read_Byte(0x06);//OUT_Y_H_A - - magX=(short)((BUF[1] << 8)|BUF[2]); //Combine MSB and LSB of X Data output register - magY=(short)((BUF[3] << 8)|BUF[4]); //Combine MSB and LSB of Y Data output register - magZ=(short)((BUF[5] << 8)|BUF[6]); //Combine MSB and LSB of Z Data output register - - magX=(magX-magX_off); - magY=(magY-magY_off); // -4096~4095 -> -8~8 - magZ=(magZ-magZ_off); - - - - } - void hmc_correct(){ - int i=0; - int num=1000; - - for(i=0;i -8~8 + magZ=(magZ-magZ_off); + + + +} + +void hmc_correct(){ + int i=0; + int num=1000; + + for(i=0;i mx) magRange0 = mx; // x min + if(magRange1 < mx) magRange1 = mx; // x max + if(magRange2 > my) magRange2 = my; + if(magRange3 < my) magRange3 = my; + if(magRange4 > mz) magRange4 = mz; // z min + if(magRange5 < mz) magRange5 = mz; // z max + magX_off = (magRange0+magRange1)/2.0; + magY_off = (magRange2+magRange3)/2.0; + magZ_off = (magRange4+magRange5)/2.0; } - - - - void hmc_get_off(float mx, float my, float mz){ - - if(magRange0 > mx) magRange0 = mx; // x min - if(magRange1 < mx) magRange1 = mx; // x max - if(magRange2 > my) magRange2 = my; - if(magRange3 < my) magRange3 = my; - if(magRange4 > mz) magRange4 = mz; // z min - if(magRange5 < mz) magRange5 = mz; // z max - magX_off = (magRange0+magRange1)/2.0; - magY_off = (magRange2+magRange3)/2.0; - magZ_off= (magRange4+magRange5)/2.0; - - } \ No newline at end of file diff --git a/project/p10-ANO/application/data_transfer.c b/project/p10-ANO/application/data_transfer.c index 230a752..35a3c63 100644 --- a/project/p10-ANO/application/data_transfer.c +++ b/project/p10-ANO/application/data_transfer.c @@ -336,7 +336,7 @@ void ANO_DT_Data_Receive_Anl(u8 *data_buf,u8 num) // ctrl_1.PID[PIDYAW].kp = 0.001*( (vs16)(*(data_buf+16)<<8)|*(data_buf+17) ); // ctrl_1.PID[PIDYAW].ki = 0.001*( (vs16)(*(data_buf+18)<<8)|*(data_buf+19) ); // ctrl_1.PID[PIDYAW].kd = 0.001*( (vs16)(*(data_buf+20)<<8)|*(data_buf+21) ); - //Param_SavePID(); + //Param_SavePID(); roll_angle_PID.P = 0.001f * ( (*(data_buf+4)<<8) |*(data_buf+5) ); roll_angle_PID.I = 0.001f * ( (*(data_buf+6)<<8) |*(data_buf+7) ); roll_angle_PID.D = 0.01f * ( (*(data_buf+8)<<8) |*(data_buf+9) ); diff --git a/project/p10-ANO/application/gpio_hmc5883.h b/project/p10-ANO/application/gpio_hmc5883.h index 965d1af..88a6e14 100644 --- a/project/p10-ANO/application/gpio_hmc5883.h +++ b/project/p10-ANO/application/gpio_hmc5883.h @@ -1,12 +1,15 @@ +#ifndef __GPIO_HMC5883_H +#define __GPIO_HMC5883_H #include // u8 define in stm32f4xx.h for legacy -extern short magX, magY, magZ; -extern float magX_off, magY_off, magZ_off; -extern float magX_cc, magY_cc, magZ_cc; -extern short magRange0,magRange1,magRange2,magRange3,magRange4,magRange5; +extern short magX, magY, magZ; +extern float magX_off, magY_off, magZ_off; +extern float magX_cc, magY_cc, magZ_cc; +extern short magRange0, magRange1, magRange2, magRange3, magRange4, magRange5; - void InitHMC5883(void); - void hmc_get_off(float mx, float my, float mz); - void hmc_correct(void); - void updateHMC5883(void); - \ No newline at end of file +void InitHMC5883(void); +void hmc_get_off(float mx, float my, float mz); +void hmc_correct(void); +void updateHMC5883(void); + +#endif /* __GPIO_HMC5883_H */ diff --git a/project/p10-ANO/application/main.c b/project/p10-ANO/application/main.c index 7e3b7bb..e11283e 100644 --- a/project/p10-ANO/application/main.c +++ b/project/p10-ANO/application/main.c @@ -52,7 +52,7 @@ int main(void) RCV_IC_init(); #endif while (mpu_dmp_init()); // 六轴数据的具体量程见：`int mpu_init(void)` 函数实现上面的注释 - InitHMC5883(); //初始化磁力计 + InitHMC5883(); //初始化磁力计 /* USART configuration */ USART_Config(); @@ -60,10 +60,10 @@ int main(void) /* Enable the MY_COM1 Receive interrupt: this interrupt is generated when the MY_COM1 receive data register is not empty */ USART_ITConfig(MY_COM1, USART_IT_RXNE, ENABLE); - PID_init(); - - - hmc_correct(); //数据校正 + PID_init(); + + + hmc_correct(); //数据校正 updateHMC5883(); yaw_angle_PID.Desired = atan2(magY,magX) * 180/3.14; for (i=0;;i++) { diff --git a/project/p10-ANO/application/pid.c b/project/p10-ANO/application/pid.c index e389b04..515a153 100644 --- a/project/p10-ANO/application/pid.c +++ b/project/p10-ANO/application/pid.c @@ -44,7 +44,7 @@ void PID_init(void) yaw_angle_PID.I = 0; yaw_angle_PID.D = 0; yaw_angle_PID.iLimit = I_limit_init; - + pitch_rate_PID.P = 1.5; pitch_rate_PID.I = 0; pitch_rate_PID.D = 1; @@ -111,12 +111,12 @@ static inline uint16_t range_thr(uint16_t thr) void CtrlAttiAng(void) { static uint32_t told = 0; - + uint32_t tnow = msTimerCounter; roll_angle_PID.Desired = range_trans(u16Rcvr_ch1, max_angle_pr); // f(u16Rcvr_ch1) + pitch_angle_PID.Desired = range_trans(u16Rcvr_ch2, max_angle_pr); // f(u16Rcvr_ch2) + yaw_angle_PID.Desired += range_trans(3000-u16Rcvr_ch4, max_rate_pr)*(tnow-told)/1000.f; - + PID_postion_cal(&roll_angle_PID, roll, tnow-told); PID_postion_cal(&pitch_angle_PID, pitch, tnow-told); PID_postion_cal(&yaw_angle_PID, yaw, tnow-told); @@ -127,7 +127,7 @@ void CtrlAttiAng(void) void CtrlAttiRate(void) { static uint32_t told = 0; - + roll_rate_PID.Desired = roll_angle_PID.Output ; pitch_rate_PID.Desired = pitch_angle_PID.Output; yaw_rate_PID.Desired = yaw_angle_PID.Output ; @@ -135,7 +135,7 @@ void CtrlAttiRate(void) //roll_rate_PID.Desired = range_trans(u16Rcvr_ch1, 100); //pitch_rate_PID.Desired = range_trans(u16Rcvr_ch2, 100); //yaw_rate_PID.Desired = range_trans(3000-u16Rcvr_ch4, 60); - + uint32_t tnow = msTimerCounter; PID_postion_cal(&roll_rate_PID, gyro[0] / DMP_GYRO_SCALE, tnow-told); // DMP_GYRO_SCALE PID_postion_cal(&pitch_rate_PID, gyro[1] / DMP_GYRO_SCALE, tnow-told); diff --git a/project/p10-ANO/application/pid.h b/project/p10-ANO/application/pid.h index 70efe4e..5176fbe 100644 --- a/project/p10-ANO/application/pid.h +++ b/project/p10-ANO/application/pid.h @@ -5,7 +5,7 @@ typedef struct { float P, I, D; // PID三个参数 - float Desired; // 期望 + float Desired; // 期望 float Error; // 误差 float PreError; // 上一次误差 float Integ; // 积分值