void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do {
SensorsRead(SENSOR_GYRO,1);
#ifndef OPTION_RC
DelayMsec(5);
#else
DelayMsec(16);
#endif
//printf("T:%d\n",getTickCount());
status=nvtGyroScaleCalibrate(axis);
led_arm_state(LED_STATE_TOGGLE);
UpdateLED();
} while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
// 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
}
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);
}
