Skip to content

Commit

Permalink
工程代码风格整理
Browse files Browse the repository at this point in the history
  • Loading branch information
@xxyyttxx
xxyyttxx committed Jan 2, 2018
1 parent dc10f5e commit 028bb22
Show file tree
Hide file tree
Showing 7 changed files with 92 additions and 94 deletions.
14 changes: 7 additions & 7 deletions project/p10-ANO/application/Attitude.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 中
}
127 changes: 61 additions & 66 deletions project/p10-ANO/application/HMC5883.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -33,8 +34,6 @@ float magX_cc, magY_cc, magZ_cc;
return 0;
}



uint8_t HMC_Read_Byte(uint8_t reg)
{
uint8_t res;
Expand All @@ -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<num;i++)
{
updateHMC5883();
hmc_get_off(magX,magY,magZ);
delay(6); //6s
}


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<num;i++)
{
updateHMC5883();
hmc_get_off(magX,magY,magZ);
delay(6); //6s
}
}

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;
}



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;

}
2 changes: 1 addition & 1 deletion project/p10-ANO/application/data_transfer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) );
Expand Down
21 changes: 12 additions & 9 deletions project/p10-ANO/application/gpio_hmc5883.h
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,15 @@
#ifndef __GPIO_HMC5883_H
#define __GPIO_HMC5883_H

#include <stdint.h> // 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);

void InitHMC5883(void);
void hmc_get_off(float mx, float my, float mz);
void hmc_correct(void);
void updateHMC5883(void);

#endif /* __GPIO_HMC5883_H */
10 changes: 5 additions & 5 deletions project/p10-ANO/application/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,18 +52,18 @@ int main(void)
RCV_IC_init();
#endif
while (mpu_dmp_init()); // 六轴数据的具体量程见：`int mpu_init(void)` 函数实现上面的注释
InitHMC5883(); //初始化磁力计
InitHMC5883(); //初始化磁力计

/* USART configuration */
USART_Config();

/* 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++) {
Expand Down
10 changes: 5 additions & 5 deletions project/p10-ANO/application/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -127,15 +127,15 @@ 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 ;

//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);
Expand Down
2 changes: 1 addition & 1 deletion project/p10-ANO/application/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
typedef struct
{
float P, I, D; // PID三个参数
float Desired; // 期望
float Desired; // 期望
float Error; // 误差
float PreError; // 上一次误差
float Integ; // 积分值
Expand Down

0 comments on commit 028bb22

Please sign in to comment.