/*
* 函数名：main
* 描述 : 主函数
* 输入 ：无
* 输出 : 无
*/
int main(void)
{
/* 配置系统时钟为 72M */
SystemInit();
LED_GPIO_Config();
Nvic_Init();
I2C_GPIO_Config();
Delay(0xFFFF);
InitMPU6050();
Delay(0xFFFF);
Init_HMC5883L();
Delay(0xFFFF);
Tim3_Init(2500);
usart1_config();
while(1)
{
if(sentFlag)
{
sentFlag = 0;
UART1_ReportIMU();
}
}
}
void READ_MPU6050(MPU6050 *p) {
s16 BUF[6];
float ACCEL_X, ACCEL_Y, ACCEL_Z, GYRO_X, GYRO_Y, GYRO_Z;
s16 Accel_X, Accel_Y, Accel_Z, Gyro_X, Gyro_Y, Gyro_Z;
InitMPU6050();
BUF[0] = (s16)GetData_Word(ACCEL_XOUT_H);
BUF[1] = (s16)GetData_Word(ACCEL_YOUT_H);
BUF[2] = (s16)GetData_Word(ACCEL_ZOUT_H);
BUF[3] = (s16)GetData_Word(GYRO_XOUT_H);
BUF[4] = (s16)GetData_Word(GYRO_YOUT_H);
BUF[5] = (s16)GetData_Word(GYRO_ZOUT_H);
ACCEL_X = (float)BUF[0] / Factor_2g;
ACCEL_Y = (float)BUF[1] / Factor_2g;
ACCEL_Z = (float)BUF[2] / Factor_2g;
GYRO_X = (float)BUF[3] / Factor_1000_Gyroscope;
GYRO_Y = (float)BUF[4] / Factor_1000_Gyroscope;
GYRO_Z = (float)BUF[5] / Factor_1000_Gyroscope;
Accel_X = (s16)(ACCEL_X * 100); //小数点一位
Accel_Y = (s16)(ACCEL_Y * 100);
Accel_Z = (s16)(ACCEL_Z * 100);
Gyro_X = (s16)(GYRO_X * 100);
Gyro_Y = (s16)(GYRO_Y * 100);
Gyro_Z = (s16)(GYRO_Z * 100);
p->Accel_X = (float)Accel_X / 100;
p->Accel_Y = (float)Accel_Y / 100;
p->Accel_Z = (float)Accel_Z / 100;
p->Gyro_X = (float)Gyro_X / 100;
p->Gyro_Y = (float)Gyro_Y / 100;
p->Gyro_Z = (float)Gyro_Z / 100;
}
void SYS_INIT(void)
{
LED_INIT(); //LED及串口IO 初始化
LED_FLASH(); //LED闪烁
Tim3_Init(500); //中断初始化 //1000=1MS,500=0.5MS
Moto_Init(); //PWM
// Uart1_Init(115200); //串口初始化，飞控上几乎无用
Spi1_Init(); //SPI初始化
Nvic_Init(); //中断初始化
Nrf24l01_Init(MODEL_TX2,40); //2401中断初始化 主发送 通道 40
// if(Nrf24l01_Check()) Uart1_Put_String("NRF24L01 IS OK !\r\n"); //检测2401是否初始化成功
// else Uart1_Put_String("NRF24L01 IS NOT OK !\r\n");
InitMPU6050();
ADC1_Init(); //检测电池电压
FLASH_Unlock(); //保存飞飞控参数
EE_INIT();
EE_READ_ACC_OFFSET();
EE_READ_GYRO_OFFSET();
EE_READ_PID();
PID_ROL.P = PID_PIT.P = 5; //用于初始化pid，如用匿名上位机写入pid，则屏蔽
PID_ROL.D = PID_PIT.D = 0.1;
PID_YAW.P = 0.5;
PID_YAW.D = 0.05;
}
void Sensor_Init(void)
{
Init_State.MPU6050_State = InitMPU6050();
Init_State.NRF2401_State = NRF24L01_Check();
NRF24L01_Mode(1);
MS5611_init();
}
int16_t main(void)
{
unsigned int buttonCounter = 20000;
int i;
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
//initSerial();
initSPI1();
initSPI2();
InitI2C();
__delay32(1600000); // allow for POR for all devices
initnRF();
ControlByte = 0x00D0; // mpu6050 address
InitMPU6050(ControlByte);
// InitHMC5883L();
__delay_ms(500);
/** Init control loop *****************************************************/
readSensorData();
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
AngleOffset[0] = accXangle;
AngleOffset[1] = accYangle;
CalibrateGyro(); // Finding the gyro zero-offset
SetupInterrupts();
////// int serStringN = 14;
////// char nRFstatus = 0;
//////// int i;
////// delaytime = 1;
////// bool mode = 0;
while(1)
{
/** Read Button RB7 for enable steppers *******************************/
if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 1;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter)
{
enableSteppers = 2;
__delay32(40000000);
buttonState = 2;
buttonCounter = 20000;
enableSteppers = 1;
}
if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 3;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter)
{
buttonState = 0;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonCounter)
{
buttonCounter--;
}
for (i=0;i<100;i++)
{
i=i;
}
// __delay32(100);
}
}
int main(void)
{
/* initialize the core clock and the systick timer */
InitClock();
InitSysTick();
/* initialize the RGB led */
LED_Init();
/* Initialize UART0 */
InitUart0();
/* double rainbow all across the sky */
DoubleFlash();
/* initialize the I2C bus */
I2C_Init();
#if DATA_FUSE_MODE
/* signaling for fusion */
FusionSignal_Init();
#endif // DATA_FUSE_MODE
/* initialize UART fifos */
RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);
/* initialize UART0 interrupts */
Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
Uart0_EnableReceiveIrq();
/* initialize I2C arbiter */
InitI2CArbiter();
/* initialize the IMUs */
InitHMC5883L();
InitMPU6050();
// InitMPU6050();
#if ENABLE_MMA8451Q
InitMMA8451Q();
#endif
/* Wait for the config messages to get flushed */
//TrafficLight();
DoubleFlash();
RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);
#if ENABLE_MMA8451Q
/* initialize the MMA8451Q data structure for accelerometer data fetching */
mma8451q_acc_t acc;
MMA8451Q_InitializeData(&acc);
#endif
/* initialize the MPU6050 data structure */
mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
MPU6050_InitializeData(&accgyrotemp);
MPU6050_InitializeData(&previous_accgyrotemp);
/* initialize the HMC5883L data structure */
hmc5883l_data_t compass, previous_compass;
HMC5883L_InitializeData(&compass);
HMC5883L_InitializeData(&previous_compass);
/* initialize HMC5883L reading */
uint32_t lastHMCRead = 0;
const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */
/************************************************************************/
/* Fetch scaler values */
/************************************************************************/
#if DATA_FUSE_MODE
const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Prepare data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
uint32_t last_transmit_time = 0;
uint32_t last_fusion_time = systemTime();
fusion_initialize();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Main loop */
/************************************************************************/
for(;;)
{
/* helper variables to track data freshness */
uint_fast8_t have_gyro_data = 0;
uint_fast8_t have_acc_data = 0;
uint_fast8_t have_mag_data = 0;
/************************************************************************/
/* Determine if sensor data fetching is required */
/************************************************************************/
/* helper variables for event processing */
int eventsProcessed = 0;
int readMPU, readHMC;
#if ENABLE_MMA8451Q
int readMMA;
#endif
/* atomic detection of fresh data */
__disable_irq();
#if ENABLE_MMA8451Q
readMMA = poll_mma8451q;
#endif
readMPU = poll_mpu6050;
poll_mma8451q = 0;
poll_mpu6050 = 0;
__enable_irq();
/* detection of HMC read */
/*
* TODO: read synchronized with MPU
*/
readHMC = 0;
uint32_t time = systemTime();
if ((time - lastHMCRead) >= readHMCEvery)
{
readHMC = 1;
lastHMCRead = time;
}
/************************************************************************/
/* Fetching MPU6050 sensor data if required */
/************************************************************************/
/* read accelerometer/gyro */
if (readMPU)
{
LED_BlueOff();
I2CArbiter_Select(MPU6050_I2CADDR);
MPU6050_ReadData(&accgyrotemp);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
|| (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
|| (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);
have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
|| (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
|| (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);
/* loop current data --> previous data */
previous_accgyrotemp = accgyrotemp;
}
/************************************************************************/
/* Fetching HMC5883L sensor data if required */
/************************************************************************/
/* read compass data */
if (readHMC)
{
I2CArbiter_Select(HMC5883L_I2CADDR);
HMC5883L_ReadData(&compass);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_mag_data = (compass.x != previous_compass.x)
|| (compass.y != previous_compass.y)
|| (compass.z != previous_compass.z);
/* loop current data --> previous data */
previous_compass = compass;
}
/************************************************************************/
/* Fetching MMA8451Q sensor data if required */
/************************************************************************/
#if ENABLE_MMA8451Q
/* read accelerometer */
if (readMMA)
{
LED_RedOff();
I2CArbiter_Select(MMA8451Q_I2CADDR);
MMA8451Q_ReadAcceleration14bitNoFifo(&acc);
/* mark event as detected */
eventsProcessed = 1;
}
#endif
/************************************************************************/
/* Raw sensor data output over serial */
/************************************************************************/
#if DATA_FETCH_MODE
/* data availability + sanity check
* This sent me on a long bug hunt: Sometimes the interrupt would be raised
* even if not all data registers were written. This always resulted in a
* z data register not being fully written which, in turn, resulted in
* extremely jumpy measurements.
*/
if (readMPU && accgyrotemp.status != 0)
{
/* write data */
uint8_t type = 0x02;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
}
/* data availability + sanity check */
if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
{
uint8_t type = 0x03;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
}
#if ENABLE_MMA8451Q
/* data availability + sanity check */
if (readMMA && acc.status != 0)
{
uint8_t type = 0x01;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
}
#endif
#endif // DATA_FETCH_MODE
/************************************************************************/
/* Sensor data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
// if there were sensor data ...
if (eventsProcessed)
{
v3d gyro, acc, mag;
// convert, calibrate and store gyroscope data
if (have_gyro_data)
{
sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
fusion_set_gyroscope_v3d(&gyro);
}
// convert, calibrate and store accelerometer data
if (have_acc_data)
{
sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
fusion_set_accelerometer_v3d(&acc);
}
// convert, calibrate and store magnetometer data
if (have_mag_data)
{
sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
fusion_set_magnetometer_v3d(&mag);
}
// get the time differential
const uint32_t current_time = systemTime();
const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));
last_fusion_time = current_time;
FusionSignal_Predict();
// predict the current measurements
fusion_predict(deltaT);
FusionSignal_Update();
// correct the measurements
fusion_update(deltaT);
FusionSignal_Clear();
#if 0
fix16_t yaw, pitch, roll;
fusion_fetch_angles(&roll, &pitch, &yaw);
#if 0
float yawf = fix16_to_float(yaw),
pitchf = fix16_to_float(pitch),
rollf = fix16_to_float(roll);
IO_SendInt16((int16_t)yawf);
IO_SendInt16((int16_t)pitchf);
IO_SendInt16((int16_t)rollf);
IO_SendByteUncommited('\r');
IO_SendByte('\n');
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
last_transmit_time = current_time;
}
#endif
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
switch (output_mode)
{
case RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION:
{
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 43;
fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION_RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 44;
fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case SENSORS_RAW:
{
uint8_t type = 0;
fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
}
last_transmit_time = current_time;
}
#endif
}
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Read user data input */
/************************************************************************/
/* as long as there is data in the buffer */
while(!RingBuffer_Empty(&uartInputFifo))
{
/* light one led */
LED_RedOn();
/* fetch byte */
uint8_t data = IO_ReadByte();
output_mode = (output_mode_t)data;
LED_RedOff();
#if 0
/* echo to output */
IO_SendByte(data);
/* mark event as detected */
eventsProcessed = 1;
#endif
}
/************************************************************************/
/* Save energy if you like to */
/************************************************************************/
/* in case of no events, allow a sleep */
if (!eventsProcessed)
{
/*
* Care must be taken with this instruction here, as it can lead
* to a condition where after being woken up (e.g. by the SysTick)
* and looping through, immediately before entering WFI again
* an interrupt would yield a true condition for the branches below.
* In this case this loop would be blocked until the next IRQ,
* which, in case of a 1ms SysTick timer, could be too late.
*
* To counter this behaviour, SysTick has been speed up by factor
* four (0.25ms).
*/
#if 0
__WFI();
#endif
}
}
return 0;
}
int main(void)
{
u16 sta , i=0;
int t = 200;
delay_init(); //延时函数初始化
LED_Init();
LED0 = 0;delay_ms(200); LED0 = 1;
LED1 = 0;delay_ms(200); LED1 = 1;
LED2 = 0;delay_ms(200); LED2 = 1;
LED3 = 0;delay_ms(200); LED3 = 1;
NVIC_Configuration(); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
uart_init(9600); //串口初始化为9600
EXTIX_Init( );
NRF24L01_Init(); //初始化NRF24L01
NRF24L01_RX_Mode();
while(NRF24L01_Check() == 1)//检查NRF24L01是否在位.
{
LED3=!LED3; printf("NRF ERROR!!\r\n");
delay_ms(100 );
}
printf("NRF okk!!\r\n");
sta=NRF24L01_Read_Reg(STATUS); //读取状态寄存器的值
NRF24L01_Write_Reg(WRITE_REG_NRF+STATUS,sta); //清除TX_DS或MAX_RT中断标志
NRF24L01_Write_Reg(FLUSH_RX,0xff);//清除RX FIFO寄存器
TIM2_PWM_Init(999,9); //PWM OUT
TIM3_Int_Init(0xffff,71); //做计时器用
IIC_Init();
InitMPU6050();
Init_HMC5883();
suanfa_GetOrigin(); //初始欧拉角
TIM4_Int_Init(49,7199); //PID调速中断 放在最后初始化，防止打断角度校准
LOCK = 1;
UN_LOCK = 0;
while(1){
if(LOCK)
{
lock();
LED0 = 0;delay_ms(500);LED0 = 1;
Power = 0;
Target_x = 0;
Target_y = 0;
}
if(UN_LOCK)
{
suanfa();
printf("%.2lf %.2lf %.2lf\r\n",EA.Roll,EA.Pitch,EA.Yaw);
// TIM_Cmd(TIM4, ENABLE);
Data_Receive_Anl();
}
// printf("--p\r\n");
}
}
