sp_simulationReturnItem simulation_main(){ #ifdef TESTS testGetTriangleDist(); return; #endif sp_simulation = initializeCarSim(); FILE *file; file = fopen("file.csv","w"); int line = 0; fprintf(file,"line,MS,x,y,angle,steeringAngle,speed,ACC,stearingCmd,PowerCmd,debug,"); fprintf(file,"GPS_x,GPS_y,Compass,Encoder_L,Encoder_R,fusion_x,fusion_y,fusion_angle\n"); simulation_print_data(sp_simulation,file, line++); fprintf(file,"0,0,0\n"); int x; int goalCounter = 0; //float x_goal[10] = {0, 30,-30,30,-30,30,-30,30}; //criss cross //float y_goal[10] = {30,30, 10,-10, -30,-10 ,-20,-30}; //criss cross float x_goal[10] = {30, 30,-30,-30, 20,20,-20,-20}; //turn left box float y_goal[10] = {-20 ,30, 30,-30,-30,20 ,20,-20}; //turn left box s_routePlanner_return cmd = gotoWaypoint(0, 0, x_goal[0], y_goal[0], 0); sp_simulation->steeringPercentAngleCmd = cmd.steeringCmd; sp_simulation->powerPercentCmd = cmd.power; sp_simulationReturnItem sp_returnData = (sp_simulationReturnItem)malloc(sizeof(s_simulationReturnItem)); for(x = 0; x<20000; x++) { simulation_update(sp_simulation); s_fusion_return fusion = sensorFusionAndMapping(); s_routePlanner_return cmd = gotoWaypoint(fusion.x, fusion.y, x_goal[goalCounter], y_goal[goalCounter], fusion.angle); sp_simulation->steeringPercentAngleCmd = cmd.steeringCmd; sp_simulation->powerPercentCmd = cmd.power; if(cmd.finished) { goalCounter++; } if(x%10==0) { sp_returnData->x[x/10] = sp_simulation->x; sp_returnData->y[x/10] = sp_simulation->y; sp_returnData->angle[x/10] = sp_simulation->angle; simulation_print_data(sp_simulation, file, line++); fprintf(file,"%f,%f,%f\n",fusion.x,fusion.y,fusion.angle); sp_returnData->fusion_x[x/10] = fusion.x; sp_returnData->fusion_y[x/10] = fusion.y; sp_returnData->fusion_angle[x/10] = fusion.angle; //printf("%f,%f,%f\n",fusion.x,fusion.y,fusion.angle); } } fclose(file); /*done!*/ return sp_returnData; }
void tasks_run_imu_update(void* arg) { if (central_data->state.mav_mode.HIL == HIL_ON) { simulation_update(¢ral_data->sim_model); } else { lsm330dlc_gyro_update(&(central_data->imu.raw_gyro)); lsm330dlc_acc_update(&(central_data->imu.raw_accelero)); hmc5883l_update(&(central_data->imu.raw_compass)); } imu_update( ¢ral_data->imu); qfilter_update(¢ral_data->attitude_filter); if (central_data->imu.calibration_level == OFF) { position_estimation_update(¢ral_data->position_estimation); } }