void runTaskCode(void *unused) {
uint32_t axis = 0;
uint32_t loops = 0;
AQ_NOTICE("Run task started\n");
while (1) {
// wait for data
CoWaitForSingleFlag(imuData.sensorFlag, 0);
// soft start GPS accuracy
runData.accMask *= 0.999f;
navUkfInertialUpdate();
// record history for acc & mag & pressure readings for smoothing purposes
// acc
runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];
runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;
runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];
// mag
runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];
runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;
runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];
// pressure
runData.sumPres -= runData.presHist[runData.sensorHistIndex];
runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
runData.sumPres += runData.presHist[runData.sensorHistIndex];
runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;
if (!((loops+1) % 20)) {
simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
else if (!((loops+7) % 20)) {
simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
}
#ifndef USE_DIGITAL_IMU
else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
}
#endif
// optical flow update
else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
navUkfFlowUpdate();
}
// only accept GPS updates if there is no optical flow
else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
CoClearFlag(gpsData.gpsPosFlag);
// refine static sea level pressure based on better GPS altitude fixes
if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
navPressureAdjust(gpsData.height);
runData.bestHacc = gpsData.hAcc;
}
}
else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
CoClearFlag(gpsData.gpsVelFlag);
}
// observe zero position
else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroPos();
}
// observer zero velocity
else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
navUkfZeroVel();
}
// observe that the rates are exactly 0 if not flying or moving
else if (!(supervisorData.state & STATE_FLYING)) {
float stdX, stdY, stdZ;
arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);
if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
if (!((axis + 0) % 3))
navUkfZeroRate(IMU_RATEX, 0);
else if (!((axis + 1) % 3))
navUkfZeroRate(IMU_RATEY, 1);
else
navUkfZeroRate(IMU_RATEZ, 2);
axis++;
}
}
navUkfFinish();
altUkfProcess(AQ_PRESSURE);
// determine which altitude estimate to use
if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
runData.altPos = &ALT_POS;
runData.altVel = &ALT_VEL;
}
else {
runData.altPos = &UKF_ALTITUDE;
runData.altVel = &UKF_VELD;
}
CoSetFlag(runData.runFlag); // new state data
navNavigate();
#ifndef HAS_AIMU
analogDecode();
#endif
if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
loggerDoHeader();
loggerDo();
gimbalUpdate();
#ifdef CAN_CALIB
canTxIMUData(loops);
#endif
calibrate();
loops++;
}
}
/**
*******************************************************************************
* @brief Set time task
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task use to set time.
*******************************************************************************
*/
void time_set(void *pdata)
{
static unsigned char bct = 0; /* Button calc */
U16 evtmp = 0;
pdata = pdata;
for (;;)
{
evtmp = CoWaitForMultipleFlags(EVT_BUTTON_SEL|EVT_BUTTON_ADD,OPT_WAIT_ANY,0,&errinfo[20]);
if(errinfo[20] != E_OK)
uart_printf("\r Flag ERROR:\n");
if (evtmp == EVT_BUTTON_SEL)
{
bct++;
switch (bct)
{
case 1:
timeflag = 1;
if(lcd_blink_id == 0)
lcd_blink_id = CoCreateTask (lcd_blink,(void *)0, LCD_BLINK_PRI ,&lcd_blink_Stk[TASK_STK_SIZE - 1], TASK_STK_SIZE );
DisableRTCInterrupt();
CoClearFlag(time_display_flg);
CoSetFlag(lcd_blink_flg);
break;
case 2:
timeflag = 2;
break;
case 3:
timeflag = 3;
break;
case 4:
bct = 0;
CoClearFlag(lcd_blink_flg);
CoSetFlag(time_display_flg);
EnableRTCInterrupt();
break;
default: break;
};
CoTickDelay(40);
CoClearFlag(button_sel_flg);
}
else if(evtmp == EVT_BUTTON_ADD)
{
CoEnterMutexSection(mut_lcd);
switch (bct)
{
case 1:
time[0]++;
if (time[0] >= 60)
time[0] = 0;
chart[6] = time[0]/10 + '0';
chart[7] = time[0]%10 + '0';
break;
case 2:
time[1]++;
if (time[1] >= 60)
time[1] = 0;
chart[3] = time[1]/10 + '0';
chart[4] = time[1]%10 + '0';
break;
case 3:
time[2]++;
if (time[2] >= 24)
time[2] = 0;
chart[0] = time[2]/10 + '0';
chart[1] = time[2]%10 + '0';
break;
default: break;
}
set_cursor(7, 0);
lcd_print (chart);
CoLeaveMutexSection(mut_lcd);
CoTickDelay(40);
CoClearFlag(button_add_flg);
}
}
}
