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);
}
