/**
*******************************************************************************
* @brief Respond the request in the service request queue.
* @param[in] None
* @param[out] None
* @retval None
*
* @par Description
* @details This function be called to respond the request in the service
* request queue.
* @note
*******************************************************************************
*/
void RespondSRQ(void)
{
#if CFG_MAX_SERVICE_REQUEST > 0
U16 i;
P_SQC pcell;
#endif
#if (CFG_TASK_WAITTING_EN > 0)
if(TimeReq == TRUE) /* Time delay request? */
{
TimeDispose(); /* Yes,call handler */
TimeReq = FALSE; /* Reset time delay request false */
}
#endif
#if CFG_TMR_EN > 0
if(TimerReq == TRUE) /* Timer request? */
{
TmrDispose(); /* Yes,call handler */
TimerReq = FALSE; /* Reset timer request false */
}
#endif
#if CFG_MAX_SERVICE_REQUEST > 0
pcell = &ServiceReq.cell[0]; /* Get the head item of service request list*/
for(i=0;i<ServiceReq.cnt;i++,pcell++)
{
switch(pcell->type) /* Judge service request type */
{
#if CFG_SEM_EN > 0
case SEM_REQ: /* Semaphore post request,call handler*/
CoPostSem(pcell->id);
break;
#endif
#if CFG_MAILBOX_EN > 0
case MBOX_REQ: /* Mailbox post request,call handler */
CoPostMail(pcell->id,pcell->arg);
break;
#endif
#if CFG_FLAG_EN > 0
case FLAG_REQ: /* Flag set request,call handler */
CoSetFlag(pcell->id);
break;
#endif
#if CFG_QUEUE_EN > 0
case QUEUE_REQ: /* Queue post request,call handler */
CoPostQueueMail(pcell->id,pcell->arg);
break;
#endif
default: /* Others,break */
break;
}
pcell->type = 0; /* Initialize the service request cell*/
pcell->id = 0;
pcell->arg = 0;
}
ServiceReq.cnt = 0; /* Initialize the service request queue */
#endif
IsrReq = FALSE;
}
void JoyTimer(void){
uint8_t key;
key=Read_Joy();
if((key!=0) && (lastjoyKey!=key)){
CoSetFlag(keyFlag[key-1]);
// CoClearFlag(keyFlag[key-1]);
}
lastjoyKey=key;
}
void serialWatch(void) {
#ifdef SERIAL_UART1_PORT
if (serialPort1 && serialAvailable(serialPort1))
CoSetFlag(serialPort1->waitFlag);
#endif
#ifdef SERIAL_UART2_PORT
if (serialPort2 && serialAvailable(serialPort2))
CoSetFlag(serialPort2->waitFlag);
#endif
#ifdef SERIAL_UART3_PORT
if (serialPort3 && serialAvailable(serialPort3))
CoSetFlag(serialPort3->waitFlag);
#endif
#ifdef SERIAL_UART4_PORT
if (serialPort4 && serialAvailable(serialPort4))
CoSetFlag(serialPort4->waitFlag);
#endif
#ifdef SERIAL_UART5_PORT
if (serialPort5 && serialAvailable(serialPort5))
CoSetFlag(serialPort5->waitFlag);
#endif
#ifdef SERIAL_UART6_PORT
if (serialPort6 && serialAvailable(serialPort6))
CoSetFlag(serialPort6->waitFlag);
#endif
}
void JoyTask (void* pdata){
uint8_t last_joyKey,key;
last_joyKey=Read_Joy();
for (;;) {
key=Read_Joy();
if((key!=0) && (last_joyKey!=key)){
CoSetFlag(keyFlag[key-1]);
// CoClearFlag(keyFlag[key-1]);
}
last_joyKey=key;
CoTimeDelay (0,0,0,50);
}
}
StatusType isr_SetFlag(OS_FlagID id)
{
if(OSSchedLock > 0) /* If scheduler is locked,(the caller is ISR) */
{
/* Insert the request into service request queue */
if(InsertInSRQ(FLAG_REQ,id,Co_NULL) == Co_FALSE)
{
return E_SEV_REQ_FULL; /* The service requst queue is full */
}
else
{
return E_OK;
}
}
else
{
return(CoSetFlag(id)); /* The caller is not ISR, set the flag*/
}
}
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 Respond the request in the service request queue.
* @param[in] None
* @param[out] None
* @retval None
*
* @par Description
* @details This function be called to respond the request in the service
* request queue.
* @note
*******************************************************************************
*/
void RespondSRQ(void) {
#if CFG_MAX_SERVICE_REQUEST > 0
SQC cell;
#endif
#if (CFG_TASK_WAITTING_EN > 0)
if(TimeReq == Co_TRUE) { /* Time delay request? */
TimeDispose(); /* Yes,call handler */
TimeReq = Co_FALSE; /* Reset time delay request Co_FALSE */
}
#endif
#if CFG_TMR_EN > 0
if(TimerReq == Co_TRUE) { /* Timer request? */
TmrDispose(); /* Yes,call handler */
TimerReq = Co_FALSE; /* Reset timer request Co_FALSE */
}
#endif
#if CFG_MAX_SERVICE_REQUEST > 0
while (ServiceReq.cnt != 0) {
IRQ_DISABLE_SAVE (); /* need to protect the following */
cell = ServiceReq.cell[ServiceReq.head]; /* extract one cell */
ServiceReq.head = (ServiceReq.head + 1) % /* move head (pop) */
CFG_MAX_SERVICE_REQUEST;
ServiceReq.cnt--;
IRQ_ENABLE_RESTORE (); /* now use the cell copy */
switch(cell.type) { /* Judge service request type */
#if CFG_SEM_EN > 0
case SEM_REQ: /* Semaphore post request,call handler*/
CoPostSem(cell.id);
break;
#endif
#if CFG_MAILBOX_EN > 0
case MBOX_REQ: /* Mailbox post request,call handler */
CoPostMail(cell.id, cell.arg);
break;
#endif
#if CFG_FLAG_EN > 0
case FLAG_REQ: /* Flag set request,call handler */
CoSetFlag(cell.id);
break;
#endif
#if CFG_QUEUE_EN > 0
case QUEUE_REQ: /* Queue post request,call handler */
CoPostQueueMail(cell.id, cell.arg);
break;
#endif
default: /* Others,break */
break;
}
}
#endif
IRQ_DISABLE_SAVE (); /* need to protect the following */
if (ServiceReq.cnt == 0) { /* another item in the queue already? */
IsrReq = Co_FALSE; /* queue still empty here */
}
IRQ_ENABLE_RESTORE (); /* now it is done and return */
}
void btPushByte(uint8_t data)
{
btTxFifo.push(data);
CoSetFlag(btFlag); // Tell the Bt task something to do
}
/**
*******************************************************************************
* @brief Initialization task
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task is called to initial hardware and created tasks.
*******************************************************************************
*/
void task_init(void *pdata)
{
uart_printf (" [OK]. \n\r\n\r");
uart_printf ("\r \"task_init\" task enter. \n\r\n\r ");
pdata = pdata;
/* Initiate Time buffer for LCD display */
chart[0] = time[2]/10 + '0';
chart[1] = time[2]%10 + '0';
chart[3] = time[1]/10 + '0';
chart[4] = time[1]%10 + '0';
chart[6] = time[0]/10 + '0';
chart[7] = time[0]%10 + '0';
uart_printf ("\r Create the \"mut_uart\" mutex... ");
mut_uart = CoCreateMutex();
if(mut_uart != E_CREATE_FAIL)
uart_printf (" [OK]. \n");
else
uart_printf (" [Fail]. \n");
uart_printf ("\r Create the \"mut_lcd\" mutex... ");
mut_lcd = CoCreateMutex();
if(mut_lcd != E_CREATE_FAIL)
uart_printf (" [OK]. \n");
else
uart_printf (" [Fail]. \n");
uart_printf ("\r Create the \"button_sel_flg\" flag... ");
/*!< Manual reset flag,initial state:0 */
button_sel_flg = CoCreateFlag(Co_FALSE,0);
if(button_sel_flg != E_CREATE_FAIL)
uart_printf (" [OK]. \n");
else
uart_printf (" [Fail]. \n");
uart_printf ("\r Create the \"button_add_flag\" flag...");
/*!< Manual reset flag,initial state:0 */
button_add_flg = CoCreateFlag(Co_FALSE,0);
if(button_add_flg != E_CREATE_FAIL)
uart_printf (" [OK]. \n\n");
else
uart_printf (" [Fail]. \n\n");
uart_printf ("\r Create the \"lcd_blink_flg\" flag... ");
lcd_blink_flg = CoCreateFlag(Co_FALSE,0); /*!< Manual reset flag,initial state:0 */
if(lcd_blink_flg != E_CREATE_FAIL)
uart_printf (" [OK]. \n");
else
uart_printf (" [Fail]. \n");
uart_printf ("\r Create the \"time_display_flg\" flag...");
/*!< Manual reset flag,initial state:0 */
time_display_flg = CoCreateFlag(Co_FALSE,0);
if(time_display_flg != E_CREATE_FAIL)
uart_printf (" [OK]. \n");
else
uart_printf (" [Fail]. \n");
/*!< Set flag to allow "time_display_flg" task run. */
CoSetFlag(time_display_flg);
uart_printf ("\r Create the first mailbox... ");
mbox0 = CoCreateMbox(EVENT_SORT_TYPE_FIFO);
if(mbox0 == E_CREATE_FAIL)
uart_printf (" [Fail]. \n\n");
else
uart_printf (" [OK]. \n\n");
/* Configure Peripheral */
uart_printf ("\r Initial hardware in Board : \n\r");
uart_printf ("\r ADC initial... ");
ADC_Configuration ();
uart_printf (" [OK]. \n");
uart_printf ("\r RTC initial... ");
RTC_Configuration ();
uart_printf (" [OK]. \n");
uart_printf ("\r GPIO initial... ");
GPIO_Configuration ();
uart_printf (" [OK]. \n");
uart_printf ("\r External interrupt initial... ");
EXIT_Configuration ();
uart_printf (" [OK]. \n");
uart_printf ("\r LCD initial... ");
LCD_Configuration ();
uart_printf (" [OK]. \n\n");
/* Create Tasks */
CoCreateTask( lcd_display_adc,
(void *)0 ,
LCD_DISPLAY_PRI ,
&lcd_display_adc_Stk[TASK_STK_SIZE-1] ,
TASK_STK_SIZE
);
CoCreateTask( uart_print ,
(void *)0 ,
UART_PRINT_PRI ,
&uart_print_Stk[TASK_STK_SIZE-1],
TASK_STK_SIZE
);
CoCreateTask( led_blink ,
(void *)0 ,
LCD_BLINK_PRI ,
&led_display_Stk[TASK_STK_SIZE-1],
TASK_STK_SIZE
);
time_display_id = CoCreateTask( time_display,
(void *)0,
TIME_DISRPLAY_PRI,
&time_display_Stk[TASK_STK_SIZE - 1],
TASK_STK_SIZE
);
CoCreateTask( time_set ,
(void *)0 ,
TIME_SET_PRI ,
&time_set_Stk[TASK_STK_SIZE-1],
TASK_STK_SIZE
);
CoExitTask(); /*!< Delete 'task_init' task. */
}
/**
*******************************************************************************
* @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);
}
}
}