void sensor_code() {
int track_tid = WhoIs("TRACKSERVER");
INTQueue sensorQ;
INTQueue sensorTimeQ;
int need_to_print = 0, last_sensor_req_time = 0, approx_hit_time = 0;
int allocated_arr[NUM_TRACED_SENSORS];
int time_arr[NUM_TRACED_SENSORS];
INTQueue_init(&sensorQ, allocated_arr, NUM_TRACED_SENSORS);
INTQueue_init(&sensorTimeQ, time_arr, NUM_TRACED_SENSORS);
char sensor_data[10] = {0};
//char old_sensor_data[10] = {0};
while(1) {
//memcpy(old_sensor_data, sensor_data, 10);
get_many_sensor(5, sensor_data, &last_sensor_req_time, &approx_hit_time);
//sensor_push(&sensorQ, &need_to_print, sensor_data, old_sensor_data, track_tid);
sensor_push2(&sensorQ, &sensorTimeQ, sensor_data, &need_to_print, &approx_hit_time, track_tid);
if (need_to_print) sensor_print(&sensorQ);
need_to_print = 0;
}
Warning(0,"Unexpected Exit() at sensor_code\n\r");
Exit();
}
void loop(){
static int i=0;
static float goffsetx=0,goffsety=0,goffsetz=0;
static float coef = 3.1415926599f/180;
if(sensor_data_ready_FLAG){
sensor_data_ready_FLAG=FALSE;
if(i<1000){
//calc gyro offset with 1000 samples
goffsetx+=local_data.gyro[0];
goffsety+=local_data.gyro[1];
goffsetz+=local_data.gyro[2];
i++;
if(i==1000){
BSP_LED_On(LED0);
goffsetx/=1000;
goffsety/=1000;
goffsetz/=1000;
}
}else{
MadgwickAHRSupdate(q, 1.0f/SAMPLINGRATE,(local_data.gyro[0]-goffsetx)*coef,(local_data.gyro[1]-goffsety)*coef,(local_data.gyro[2]-goffsetz)*coef,local_data.acc[0],local_data.acc[1],local_data.acc[2],local_data.mag[1]*-1,local_data.mag[0]*-1,local_data.mag[2]);
}
sensor_print(NULL);
//static char tmpbuf[30];
/*memset(tmpbuf,0,30);
sprintf(tmpbuf,"gyro %.2f %.2f %.2f ",data->gyro[0],data->gyro[1],data->gyro[2]);
UART_Transmit((uint8_t *)tmpbuf,30);
memset(tmpbuf,0,30);
sprintf(tmpbuf,"acc %.2f %.2f %.2f\n",data->acc[0],data->acc[1],data->acc[2]);
UART_Transmit((uint8_t *)tmpbuf,30);
memset(tmpbuf,0,30);
sprintf(tmpbuf,"mag %.2f %.2f %.2f\n",data->mag[0],data->mag[1],data->mag[2]);
UART_Transmit((uint8_t *)tmpbuf,30);*/
}
}
