void wxStreamBuffer::PutChar(char c) { wxOutputStream *outStream = GetOutputStream(); wxCHECK_RET( outStream, _T("should have a stream in wxStreamBuffer") ); // if we don't have buffer at all, just forward this call to the stream, if ( !HasBuffer() ) { outStream->OnSysWrite(&c, sizeof(c)); } else { // otherwise check we have enough space left if ( !GetDataLeft() && !FlushBuffer() ) { // we don't SetError(wxSTREAM_WRITE_ERROR); } else { PutToBuffer(&c, sizeof(c)); m_stream->m_lastcount = 1; } } }
size_t wxStreamBuffer::Write(const void *buffer, size_t size) { wxASSERT_MSG( buffer, _T("Warning: Null pointer is about to be send") ); if (m_stream) { // lasterror is reset before all new IO calls m_stream->Reset(); } size_t ret; if ( !HasBuffer() && m_fixed ) { wxOutputStream *outStream = GetOutputStream(); wxCHECK_MSG( outStream, 0, _T("should have a stream in wxStreamBuffer") ); // no buffer, just forward the call to the stream ret = outStream->OnSysWrite(buffer, size); } else // we [may] have a buffer, use it { size_t orig_size = size; while ( size > 0 ) { size_t left = GetBytesLeft(); // if the buffer is too large to fit in the stream buffer, split // it in smaller parts // // NB: If stream buffer isn't fixed (as for wxMemoryOutputStream), // we always go to the second case. // // FIXME: fine, but if it fails we should (re)try writing it by // chunks as this will (hopefully) always work (VZ) if ( size > left && m_fixed ) { PutToBuffer(buffer, left); size -= left; buffer = (char *)buffer + left; if ( !FlushBuffer() ) { SetError(wxSTREAM_WRITE_ERROR); break; } m_buffer_pos = m_buffer_start; } else // we can do it in one gulp { PutToBuffer(buffer, size); size = 0; } } ret = orig_size - size; } if (m_stream) { // i am not entirely sure what we do this for m_stream->m_lastcount = ret; } return ret; }
/* *perform kalman filter at time K, estimate best navigation parameter error at time K *estimate navigation parameters at current time when acc bias estimation is stable */ void vIEKFProcessTask(void* pvParameters) { u8 i,j; u8 acc_bias_stable = 0; //indicate whether acc bias is stably estimated char logData[100]={0}; ekf_filter filter; float dt; float measure[5]={0}; float insBufferK[INS_FRAME_LEN]; float insBufferCur[INS_FRAME_LEN]; float *p_insBuffer; float direction; u8 temp; GPSDataType gdt={0.0,0.0,0.0,0.0,0.0}; portBASE_TYPE xstatus; PosConDataType pcdt; //position message send to flight control task /*initial filter*/ filter=ekf_filter_new(9,5,(float *)iQ,(float *)iR,INS_GetA,INS_GetH,INS_aFunc,INS_hFunc); memcpy(filter->x,x,filter->state_dim*sizeof(float)); memcpy(filter->P,iP,filter->state_dim*filter->state_dim*sizeof(float)); /*capture an INS frame*/ xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY); /*last number in buffer represent time interval, not time */ p_insBuffer[INDEX_DT]=0.0; Blinks(LED1,4); for(;;) { /*capture an INS frame*/ xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY); PutToBuffer(p_insBuffer); p_insBuffer[INDEX_DT]=0.0; ReadBufferBack(insBufferK); dt=insBufferK[INDEX_DT]; /*do INS integration at time K*/ INS_Update(navParamK, insBufferK); /*do INS integration at current time*/ if(acc_bias_stable) { ReadBufferFront(insBufferCur); INS_Update(navParamCur,insBufferCur); } /*predict navigation error at time K*/ EKF_predict(filter , (void *)(insBufferK+3) , NULL , (void *)(&dt) , (void *)(filter->A) , NULL); xstatus=xQueueReceive(xUartGPSQueue,&gdt,0); //get measurement data if(xstatus == pdPASS) { float meas_Err[5]={0.0}; direction = gdt.COG*0.0174533; temp=(u8)(gdt.Lati*0.01); measure[0]=(0.01745329*(temp+(gdt.Lati-temp*100.0)*0.0166667)-initPos[0])*6371004; temp=(u8)(gdt.Long*0.01); measure[1]=(0.01745329*(temp+(gdt.Long-temp*100.0)*0.0166667)-initPos[1])*4887077; measure[2]=gdt.Alti-initPos[2]; measure[3]=gdt.SPD*0.51444*arm_cos_f32(direction); measure[4]=gdt.SPD*0.51444*arm_sin_f32(direction); for(i=0;i<5;i++) { meas_Err[i] = navParamK[i] - measure[i]; } /*data record*/ /*uncomment this to record data for matlab simulation*/ // sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n", // insBufferK[0],insBufferK[1],insBufferK[2], // insBufferK[3],insBufferK[4],insBufferK[5], // insBufferK[6],insBufferK[7], // measure[0],measure[1],measure[2],measure[3],measure[4], // navParamK[3],navParamK[4]); // xQueueSend(xDiskLogQueue,logData,0); sprintf(logData, "%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n", measure[0],measure[1],measure[3],measure[4], navParamK[0],navParamK[1],navParamK[3],navParamK[4], navParamCur[0],navParamCur[1],navParamCur[3],navParamCur[4]); xQueueSend(xDiskLogQueue,logData,0); /*update*/ EKF_update(filter , (void *)meas_Err , NULL , NULL , (void *)(filter->x) , NULL); // printf("%.2f %.2f %.4f %.4f %.4f\r\n",meas_Err[0],meas_Err[1],filter->x[6],filter->x[7],filter->x[8]); /* *correct navParamCur */ if(acc_bias_stable) { for(i=0;i<6;i++) navParamCur[i] -= filter->x[i]; navParamCur[6] = filter->x[6]; navParamCur[7] = filter->x[7]; navParamCur[8] = filter->x[8]; pcdt.posX = navParamCur[0]; pcdt.posY = navParamCur[1]; pcdt.posZ = navParamCur[2]; pcdt.veloX = navParamCur[3]; pcdt.veloY = navParamCur[4]; pcdt.veloZ = navParamCur[5]; /*put to queue*/ xQueueSend(INSToFlightConQueue,&pcdt,0); } /*correct navParameters at time K *reset error state x*/ for(i=0;i<6;i++) { navParamK[i] -= filter->x[i]; filter->x[i] = 0.0; } navParamK[6] = filter->x[6]; navParamK[7] = filter->x[7]; navParamK[8] = filter->x[8]; /*when acc bias stable, *calculate current navigation parameters*/ if(!acc_bias_stable && filter->P[60]<0.007) { acc_bias_stable = 1; //set init value for(i=0;i<filter->state_dim;i++) { navParamCur[i] = navParamK[i]; } //extrapolate current navigation parameters from time K for(i=0;i<GPS_DELAY_CNT;i++) { if(i+buffer_header >= GPS_DELAY_CNT) { for(j=0;j<INS_FRAME_LEN;j++) insBufferCur[j] = IMU_delay_buffer[(i+buffer_header-GPS_DELAY_CNT)*INS_FRAME_LEN+j]; } else { for(j=0;j<INS_FRAME_LEN;j++) insBufferCur[j] = IMU_delay_buffer[(i+buffer_header)*INS_FRAME_LEN+j]; } insBufferCur[0] -= navParamK[6]; insBufferCur[1] -= navParamK[7]; insBufferCur[2] -= navParamK[8]; INS_Update(navParamCur,insBufferCur); } Blinks(LED1,1); } // printf("%.1f %.1f %.1f %.1f %.1f\r\n",navParamK[0],navParamK[1],navParamK[2],navParamK[3],navParamK[4]); } else { /*data record*/ sprintf(logData,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n", insBufferK[0],insBufferK[1],insBufferK[2], insBufferK[3],insBufferK[4],insBufferK[5], insBufferK[6],insBufferK[7], 0.0,0.0,0.0,0.0,0.0, navParamK[3],navParamK[4]); xQueueSend(xDiskLogQueue,logData,0); } } }
/* * fill INS_delay_buffer * wait GPS signal */ void vINSAligTask(void* pvParameters) { char printf_buffer[100]; /*odometry sensor data*/ float direction; u8 temp; float *p_insBuffer; GPSDataType gdt; u16 GPS_validate_cnt=0; float uw_height; portBASE_TYPE xstatus; /*Enable ultrasonic sensor TIMER*/ TIM2_Config(); TIM2_IT_Config(); /**/ xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY); //capture an INS frame p_insBuffer[INDEX_DT]=0.0; //the last number in buffer represent time interval, not time Blinks(LED1,2); #ifdef INS_DEBUG /*GPS data is not needed in debug mode*/ while(1) { /*receive ins data and fill the IMU_delay_buffer*/ xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY); PutToBuffer(p_insBuffer); /*clear time interval*/ p_insBuffer[INDEX_DT]=0.0; /*INS_delay_buffer is full filled*/ if(buffer_header == 0) break; } navParamK[0] = 0.0; navParamK[1] = 0.0; navParamK[2] = 0.0; navParamK[3] = 0.0; navParamK[4] = 0.0; navParamK[5] = 0.0; navParamK[6] = 0.0; navParamK[7] = 0.0; navParamK[8] = 0.0; x[0]=0.0; x[1]=0.0; x[2]=0.0; x[3]=0.0; x[4]=0.0; x[5]=0.0; x[6]=0.0; x[7]=0.0; x[8]=0.0; #else //normol mode /*wait while GPS signal not stable*/ while(GPS_validate_cnt<=100) { xstatus = xQueueReceive(xUartGPSQueue,&gdt,0); if(xstatus == pdPASS) { GPS_validate_cnt ++; } if(GetUltraSonicMeasure(&uw_height)) { sprintf(printf_buffer,"%1.3f\r\n",uw_height); UartSend(printf_buffer,7); } /*receive ins data and fill the IMU_delay_buffer*/ xQueueReceive(AHRSToINSQueue,&p_insBuffer,portMAX_DELAY); PutToBuffer(p_insBuffer); /*clear time interval*/ p_insBuffer[INDEX_DT]=0.0; } /************initialize navParamK*********************/ temp=(u8)(gdt.Lati*0.01); initPos[0]=0.01745329*(temp+(gdt.Lati-temp*100.0)*0.0166667); temp=(u8)(gdt.Long*0.01); initPos[1]=0.01745329*(temp+(gdt.Long-temp*100.0)*0.0166667); initPos[2]=gdt.Alti; if(gdt.type != GPGMV) { direction = gdt.COG*0.0174533; gdt.speedN = gdt.SPD*0.51444*arm_cos_f32(direction); gdt.speedE = gdt.SPD*0.51444*arm_sin_f32(direction); } navParamK[0] = 0.0; navParamK[1] = 0.0; navParamK[2] = 0.0; navParamK[3] = gdt.speedN; navParamK[4] = gdt.speedE; navParamK[5] = 0.0; navParamK[6] = 0.0; navParamK[7] = 0.0; navParamK[8] = 0.0; /*initialize filter state param x*/ x[0]=0.0; x[1]=0.0; x[2]=0.0; x[3]=0.0; x[4]=0.0; x[5]=0.0; x[6]=0.0; x[7]=0.0; x[8]=0.0; #endif xstatus=xTaskCreate(vIEKFProcessTask,(signed portCHAR *)"ins_ekf",configMINIMAL_STACK_SIZE+1024,(void *)NULL,tskIDLE_PRIORITY+1,NULL); if(xstatus!=pdTRUE) { sprintf(printf_buffer, "failed to initialize\r\n"); UartSend(printf_buffer, strlen(printf_buffer)); } vTaskDelete(NULL); }