int main(void) {
DDRB = 0Xff;
DDRD = 0xff;
MPU6050_initialize();
USART_initialize();
//int data;
//data = read_byte_from_slave(0x68,0x1B);
//USART_transmit_byte(data);
//data = read_byte_from_slave(0x68,0x1C);
//USART_transmit_byte(data);
while(1) {
//USART_transmit_byte('\xff');
//_delay_ms(1000);
//PORTD ^= (1<<5);
}
return(0);
}
void SensorInitGYRO()
{
float Cal[GYRO_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.GYRO_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.GYRO_Done = MPU6050_initialize();
SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#else
#endif
}
if(SensorInitState.GYRO_Done) {
printf("GYRO connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
if(FlashValid) {
CalFlashState.GYRO_FLASH = true;
GyroOffset[0] = Cal[0];
GyroOffset[1] = Cal[1];
GyroOffset[2] = Cal[2];
GyroScale[0] = Cal[3];
GyroScale[1] = Cal[4];
GyroScale[2] = Cal[5];
printf("GYRO calibration from [FLASH]\n");
}
else {
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
GyroScale[0] = 1.0;
GyroScale[1] = 1.0;
GyroScale[2] = 1.0;
printf("GYRO calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
printf("Scale: %f %f %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
nvtSetGyroScale(GyroScale);
nvtSetGyroOffset(GyroOffset);
#if defined(MPU6050) || defined(MPU6500)
nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
#else
nvtSetGYRODegPLSB(calcGyro(1));
#endif
}
else
printf("GYRO connect - [FAIL]\n");
}
void SensorInitGYRO()
{
float Cal[GYRO_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.GYRO_Done)
{
#ifdef MPU6050
SensorInitState.GYRO_Done = MPU6050_initialize();
SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#endif
}
if(SensorInitState.GYRO_Done)
{
printf("GYRO connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
if(FlashValid)
{
CalFlashState.GYRO_FLASH = true;
GyroOffset[0] = Cal[0];
GyroOffset[1] = Cal[1];
GyroOffset[2] = Cal[2];
GyroScale[0] = Cal[3]*IMU_DEG_PER_LSB_CFG;
GyroScale[1] = Cal[4]*IMU_DEG_PER_LSB_CFG;
GyroScale[2] = Cal[5]*IMU_DEG_PER_LSB_CFG;
printf("GYRO calibration - [FLASH]\n");
}
else
{
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
GyroScale[0] = IMU_DEG_PER_LSB_CFG;
GyroScale[1] = IMU_DEG_PER_LSB_CFG;
GyroScale[2] = IMU_DEG_PER_LSB_CFG;
printf("GYRO calibration - [DEF]\n");
}
//printf("Offset: %f %f %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
//printf("Scale: %f %f %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
nvtSetGyroScale(GyroScale);
nvtSetGyroOffset(GyroOffset);
nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
}
else
printf("GYRO connect - [FAIL]\n");
}
/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.ACC_Done)
{
#ifdef MPU6050
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done)
{
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid)
{
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
printf("ACC calibration - [FLASH]\n");
}
else
{
AccOffset[0] = ACC_OFFSET_X;//0;
AccOffset[1] = ACC_OFFSET_Y;//0;
AccOffset[2] = ACC_OFFSET_Z;//0;
AccScale[0] = ACC_SCALE_X;//IMU_G_PER_LSB_CFG;
AccScale[1] = ACC_SCALE_Y;//IMU_G_PER_LSB_CFG;
AccScale[2] = ACC_SCALE_Z;//IMU_G_PER_LSB_CFG;
printf("ACC calibration - [DEF]\n");
}
//printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
//printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
}
else
printf("ACC connect - [FAIL]\n");
}
void setup()
{
uint8_t i=0;
//初始化系统时钟
setupSystemClock();
//初始化串口
setupUART();
UART_NVIC_INIT();
//初始化System_tick
setup_system_tick(SYSTEM_TICK_FREQ);
//初始化IIC
I2C_Init();
//初始化FLASH
FlashInit();
LoadParamsFromFlash();
//初始化低电压检测
BatteryCheckInit();
//初始化遥控
Comm_Init();
//初始化LED
LED_Init();
//初始化SENSOR
#ifdef IMU_SW //软件姿态解算
// MPU6050_initialize();
// DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
#else
MPU6050_initialize();
DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
MPU6050_DMP_Initialize(); //初始化DMP引擎
#endif
//初始化自稳定
// LED_ON();
// //测试用，延迟启动时间
// for(i=0;i<6;i++)
// {
// DelayMsec(1000);
// LED_OFF();
// }
//初始化电机
Motor_Init();
//printf("Motor_Init(); \n");
//IMU_Init(); // sample rate and cutoff freq. sample rate is too low now due to using dmp.
// printf("\n\nCPU @ %dHz\n", SystemCoreClock);
//MotorPwmOutput(100,100,0,0);
}
int main(void)
{
u8 i;
u8 RecvBuf[32];
u8 SendBuf[32];
u8 offline = 0;
u8 recv_flag = 0;
u32 pd2ms=0,pd20ams=0,pd20bms=0,pd100ms=0;
SystemInit();
RCC_Configuration();
NVIC_Configuration();
GPIO_Configuration();
USART1_Configuration();
dbgPrintf(" Init Ticktack !\r\n");
cycleCounterInit();
SysTick_Config(SystemCoreClock / 1000);
for(i=0;i<2;i++)
{
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
}
FilterInit();
controllerInit();
dbgPrintf(" Init eeprom!\r\n");
FLASH_Unlock();
EE_Init();
EE_Read_ACC_GYRO_Offset();
/* 如果PID丢失或者错误，将下面两行注释去掉，重新编译烧写，运行一遍，可将PID还原，然后重新注释，再烧写一遍 */
//EE_Write_PID();
//EE_Write_Rate_PID();
EE_Read_PID();
EE_Read_Rate_PID();
dbgPrintf(" Init adc!\r\n");
ADC_DMA_Init();
dbgPrintf(" Init NRF24L01 !\r\n");
SPI_NRF_Init();
Nrf24l01_Init();
NRF24l01_SetRxMode();
while(Nrf24l01_Check())
{
dbgPrintf("NRF24L01 Fault !\r\n");
delay_ms(500);
}
dbgPrintf("NRF24L01 Is Detected !\r\n");
dbgPrintf("Init MPU6050...\r\n");
IIC_Init();
MPU6050_initialize();
dbgPrintf("Init Motor...\r\n");
Motor_Init();
Motor_SetPwm(0,0,0,0);
pd20bms = TIMIRQCNT + 10*ITS_PER_MS;
while(1)
{
if(TIMIRQCNT>pd2ms + 2*ITS_PER_MS-1) // every 4ms
{
GetEulerAngle();
if(lock_flag==UNLOCK)
AttitudeToMotors(angle.y,angle.x,angle.z);
else
{
MOTOR1=0;
MOTOR2=0;
MOTOR3=0;
MOTOR4=0;
}
Motor_SetPwm(MOTOR1,MOTOR2,MOTOR3,MOTOR4);
pd2ms = TIMIRQCNT;
}
if(TIMIRQCNT>pd20ams + 20*ITS_PER_MS-1) // every 20ms
{
if(NRF24l01_Recv(RecvBuf)>10)
{
if((RecvBuf[RecvBuf[2]+3]==CheckSum(RecvBuf, RecvBuf[2]+3))&&(RecvBuf[0]==0xAA))
{
if(RecvBuf[1]!=0xC0)
OP_LED1;
offline=0;
switch(RecvBuf[1])
{
case 0xC0: //control
Getdesireddata(RecvBuf);
OP_LED2;
break;
case 0x10: //W PID
SetPID(RecvBuf);
break;
case 0x11: //W Attitude
SetAccGyroOffset(RecvBuf);
break;
case 0x12: //W Control offset
break;
case 0x14: //W Rate PID
SetRatePID(RecvBuf);
break;
case 0x20: //R PID
recv_flag = RESEND;
GetPID(SendBuf);
break;
case 0x21: //R Attitude
recv_flag = RESEND;
GetAccGyroOffset(SendBuf);
break;
case 0x22: //R Control offset
break;
case 0x24: //R Rate PID
recv_flag = RESEND;
GetRatePID(SendBuf);
break;
case 0x40: //校准姿态
EnableCalibration();
break;
case 0x41: //校准遥控器零点
break;
default:
break;
}
}
}
pd20ams = TIMIRQCNT;
}
if(TIMIRQCNT>pd20bms + 20*ITS_PER_MS-1) // every 20ms
{
if(recv_flag==0)
GetState(SendBuf);
else
recv_flag--;
NRF_SendData(SendBuf);
OP_LED3;
pd20bms = TIMIRQCNT;
}
if(TIMIRQCNT>pd100ms + 100*ITS_PER_MS-1) // every 100ms
{
if(offline>20)
lock_flag = LOCK;
offline++;
// OP_LED4;
pd100ms = TIMIRQCNT;
}
}
}
/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
#if defined(LSM6DS3)
status_t status;
#endif
if(!SensorInitState.ACC_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#else
LSM6DS3_init();
status = begin();
if(status==0)
SensorInitState.ACC_Done = true;
else
SensorInitState.ACC_Done = false;
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done) {
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid) {
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
AccRotate[0] = Cal[6];
AccRotate[1] = Cal[7];
AccRotate[2] = Cal[9];
AccRotate[3] = Cal[9];
AccRotate[4] = Cal[10];
AccRotate[5] = Cal[11];
AccRotate[6] = Cal[12];
AccRotate[7] = Cal[13];
AccRotate[8] = Cal[14];
printf("ACC calibration from - [FLASH]\n");
}
else {
AccOffset[0] = 0;
AccOffset[1] = 0;
AccOffset[2] = 0;
AccScale[0] = IMU_G_PER_LSB_CFG;
AccScale[1] = IMU_G_PER_LSB_CFG;
AccScale[2] = IMU_G_PER_LSB_CFG;
AccRotate[0] = 1;
AccRotate[1] = 0;
AccRotate[2] = 0;
AccRotate[3] = 0;
AccRotate[4] = 1;
AccRotate[5] = 0;
AccRotate[6] = 0;
AccRotate[7] = 0;
AccRotate[8] = 1;
printf("ACC calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
printf("M[0][1][2]: %f %f %f\n", AccRotate[0], AccRotate[1], AccRotate[2]);
printf("M[3][4][5]: %f %f %f\n", AccRotate[3], AccRotate[4], AccRotate[5]);
printf("M[6][7][8]: %f %f %f\n", AccRotate[6], AccRotate[7], AccRotate[8]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccRotate(AccRotate);
#if defined(MPU6050) || defined(MPU6500)
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
#else
nvtSetAccG_PER_LSB(calcAccel(1)/*IMU_G_PER_LSB_CFG*/);
#endif
}
else {
__disable_irq();
SYS_UnlockReg();
SYS_ResetChip();
printf("ACC connect - [FAIL]\n");
}
}