예제 #1
0
// Main Control loop
void loop()
{
	static uint32_t nextTick = 0;
	while(getTickCount()<nextTick);
	nextTick = getTickCount()+TICK_FRAME_PERIOD;
	CommandProcess();
#ifdef GPS
  GPSCommandProcess();
#endif
	SensorsRead(SENSOR_ACC|SENSOR_GYRO|SENSOR_MAG|SENSOR_BARO,1);
#ifdef ABROBOT
 SensorsRead(SENSOR_HALL,1);
#endif
#ifdef OPTION_RC
	if(IsSSVConnected()) {
	ssv_rc_update();
	if(getTickCount()%1000)
		UpdateBattery();
	}
	if(ChronographRead(ChronRC)>= OUTPUT_RC_INTERVAL) {
		SensorsDynamicCalibrate(SENSOR_GYRO|SENSOR_MAG);
		ChronographSet(ChronRC);
		computeRC();
		armDetect();
	}
#endif
	if(getMagMode()||!(GetSensorCalState()&(1<<MAG)))
		nvtUpdateAHRS(SENSOR_ACC|SENSOR_GYRO|SENSOR_MAG);
	else
		nvtUpdateAHRS(SENSOR_ACC|SENSOR_GYRO);

	if((GetFrameCount()%18)==0)
		report_sensors();
	
	IncFrameCount(1);
#ifdef OPTION_RC
	if(GetFrameCount()==MOTORS_ESC_DELAY)
		motorsStart();
	stabilizer();
	if((GetFrameCount()%12)==0)
		UpdateLED();
#endif
}
예제 #2
0
파일: main.c 프로젝트: yanyu130/JRW_4
void loop()
{
		static uint32_t nextTick = 0;
		while(millis()<nextTick){}
		nextTick = millis()+TICK_FRAME_PERIOD;	//循环间隔FRAME
	
		//处理蓝牙命令
		CommandProcess();
			
		//读取遥控命令
		Comm_Process();
	
		if(GetFrameCount()%10 == 0)
		{
			//读取姿态传感器数据

			//读取欧拉角
			#ifdef IMU_SW												//软件姿态解算
				//IMUSO3Thread();
			#else
				DMP_Routing();	//DMP 线程  所有的数据都在这里更新
			#endif
			
			//imu校准
//			if(imuCaliFlag)
//			{
//					if(IMU_Calibrate())
//					{
//						imuCaliFlag=0;
//						gParamsSaveEEPROMRequset=1;	//请求记录到EEPROM
//						imu.caliPass=1;
//					}
//			}
				
			//PID二环角速度
			//CtrlAttiRate();
			//控制电机
			//CtrlMotor();
		}

		if(GetFrameCount()%20 == 0)
		{
			//处理遥控数据
			
			//PID一环角度控制
			//CtrlAttiAng();
		}
		
		//10HZ,每100ms一次
		//if(getTickCount()%100 == 0)
		
		if(GetFrameCount()%1000 == 0)
		{
			//检测电池电量
			BatteryCheck();
			//printf("Convert result is %d\n", GetBatteryAD());
			//遥控通信丢失处理
			
			//更新LED灯状态
			UpdateLED();
		}
		
		if(GetFrameCount()%100 == 0)
		{
			//printf("yaw=%d, roll=%d, pitch=%d \n",(int)imu.yaw, (int)imu.roll, (int)imu.pitch);
			//printf("\n");
		}
		
		//故障保护
		if(GetFrameCount() > 6000 && GetFrameCount() < 8000)
		{
			Motor_Start();
			MotorPwmOutput(0,0,85,0);
		}
//		else if(GetFrameCount() >= 8000  && GetFrameCount() < 10000)
//		{
//			MotorPwmOutput(40,40,40,40);
//		}
//		else if(GetFrameCount() >= 10000   && GetFrameCount() < 12000)
//		{
//			MotorPwmOutput(60,60,60,60);
//		}
//		else if(GetFrameCount() >= 12000  && GetFrameCount() < 14000)
//		{
//			MotorPwmOutput(80,80,80,80);
//		}
//		else if(GetFrameCount() >= 14000 && GetFrameCount() < 16000)
//		{
//			MotorPwmOutput(100,100,100,100);
//		}
		else if(GetFrameCount() >= 40000 && !falg)
		{
			falg = true;
			//MotorPwmOutput(0,0,0,0);
			Motor_Stop();
		}
		
		IncFrameCount(1);
}