int sbSimpleInitialise(SBApiSimpleContext *context) { int res; int commSpeed; const SBVersionStatus*localVersion; #ifdef WIN32 DWORD tid; #endif sbInitialiseVersion(0); // no controller on this end of the link localVersion = sbGetCompiledVersion(); context->localVersion.apiVersion = localVersion->apiVersion; #ifdef WIN32 strncpy_s(context->localVersion.compileTime,SB_COMPILE_TIME_LENGTH-1, localVersion->compileTime,SB_COMPILE_TIME_LENGTH-1); #endif #ifdef LINUX strncpy(context->localVersion.compileTime, localVersion->compileTime,SB_COMPILE_TIME_LENGTH-1); #endif context->localVersion.compileTime[SB_COMPILE_TIME_LENGTH-1] = 0; // just in case printf("Local API Version\n"); sbPrintVersion(stdout,localVersion); // printf("Initialising context %p\n",context); #ifdef LINUX signal(SIGINT,sighdl); #endif #ifdef WIN32 SetConsoleCtrlHandler(sighdl,TRUE); #endif res = sbRegisterStateCallback(&context->control,stateCallback,context); commSpeed = context->commSpeed; #ifdef LINUX switch (context->commSpeed) { case 9600: commSpeed = B9600; break; case 38400: commSpeed = B38400; break; case 57600: commSpeed = B57600; break; case 115200: commSpeed = B115200; break; default: break; } #endif res = -1; switch (context->commType) { case SB_COMMTYPE_SERIAL: printf("Connecting to port %s speed %d\n", context->device,context->commSpeed); #if defined(LINUX) DEBUG(res = sbOpenSerialCommunication(&context->control,context->device,commSpeed,0)); #endif if (res) return res; break; case SB_COMMTYPE_SERIAL_RTSCTS: printf("Connecting to port %s speed %d with flow control\n", context->device,context->commSpeed); #if defined(LINUX) DEBUG(res = sbOpenSerialCommunication(&context->control,context->device,commSpeed,1)); #endif if (res) return res; break; case SB_COMMTYPE_BTWIN: printf("Connecting to btwin port %s speed %d\n", context->device,context->commSpeed); #ifdef WIN32 DEBUG(res = sbOpenBluetoothCommunication(&context->control,context->device,commSpeed)); #endif if (res) return res; break; case SB_COMMTYPE_UDP: printf("Connecting to host %s port %d\n", context->device,context->commPort); #ifdef LINUX // commSpeed is the port, device is the hostname DEBUG(res = sbOpenSocketCommunication(&context->control,context->device,context->commPort)); #endif #ifdef WIN32 // commSpeed is the port, device is the hostname DEBUG(res = sbOpenWinsockCommunication(&context->control,context->device,context->commPort)); #endif if (res) return res; break; default: printf("Invalid communication type\n"); return -1; } if (context->commEstablishedFunc) { context->commEstablishedFunc(context); } else { #if 0 // this can be useful because the initial bluetooth negociation can // leave the uart in a weird state, on the e-puck printf("Channel connected, reset platform and press enter to continue\n"); getchar(); #endif } CRITICAL(res = sbGetVersionInformation(&(context->control),&context->remoteVersion)); printf("Remote API Version\n"); sbPrintVersion(stdout,&context->remoteVersion); if (context->remoteVersion.apiVersion != context->localVersion.apiVersion) { printf("Inconsistent API Version: local %04X remote %04X\n", context->localVersion.apiVersion, context->remoteVersion.apiVersion); return -1; } if (context->masterMode) { CRITICAL(res = sbConnect(&context->control)); } if (context->sensorList) { CRITICAL(res = sbGetSensorList(&context->control, context->sensorList)); printf("Sensor list:"); sbContentPrint(stdout,*context->sensorList); printf("\n"); } if (context->masterMode) { CRITICAL(res = sbConfigureTimeout(&context->control, context->ctrlTimeout, context->cmdTimeout )); CRITICAL(res = sbConfigureOAMode(&context->control, context->oaMode)); CRITICAL(res = sbConfigureRawControl(&context->control, context->speedProfile1, context->speedProfile2)); CRITICAL(res = sbConfigureControl(&context->control, context->rollCtrlMode, context->pitchCtrlMode, context->yawCtrlMode, context->altCtrlMode)); } // Defaultc communication configuration, only 5 seconds of messages are // requested CRITICAL(res = sbConfigureComm(&context->control, context->commMode, context->commFreq, 5 * context->commFreq, context->commContent )); #ifdef WIN32 /* TODO: Error checking */ context->tid = CreateThread(NULL,0,recvthread,context,0,&tid); #endif #ifdef LINUX DEBUG(res = pthread_create(&context->tid,NULL,recvthread,context)); #endif context->initialised = 1; if (context->masterMode) { DEBUG(res = sbSimpleReachNavState(context, context->initNavState, 5.0)); } switch (context->commMode) { case SB_COM_CONTINUOUS: DEBUG(res = sbSimpleWaitState(context,&context->state,2.0)); break; case SB_COM_ONREQUEST: sbLockCommunication(&context->control); DEBUG(res = sbRequestState(&context->control,SBS_MODES,&context->state)); sbUnlockCommunication(&context->control); break; default: res = -1; } if (!res) { printf("Received first state, ready to go!\n"); } return res; }
void sbStatePrint(FILE * fp, const SBHeliState *hs) { fprintf(fp,"Content: "); sbContentPrint(fp,hs->content); fprintf(fp,"\n"); sbErrorPrint(fp,hs->errorFlags); if (TESTCONTENT(hs->content,SBS_MODES)) { fprintf(fp,"Modes:\n"); fprintf(fp,"\tNavigation %s",sbNavModeString(hs->mode.navigation)); fprintf(fp,"\tCommunication %s",sbCommModeString(hs->mode.communication)); fprintf(fp,"\tObst. Avoid: %s\n",sbOAModeString(hs->mode.oavoid)); fprintf(fp,"\tAxis: Roll %s",sbCtrlModeString(hs->mode.rollAxis)); fprintf(fp,"\tPitch %s",sbCtrlModeString(hs->mode.pitchAxis)); fprintf(fp,"\tYaw %s",sbCtrlModeString(hs->mode.yawAxis)); fprintf(fp,"\tAlti. %s",sbCtrlModeString(hs->mode.altAxis)); fprintf(fp,"\n"); } if (TESTCONTENT(hs->content,SBS_TIMESTAMP)) { fprintf(fp,"Timestamp: %8ld\n",hs->timeStamp); } if (TESTCONTENT(hs->content,SBS_RPY)) { fprintf(fp,"RPY (deg): %+6.2f, %+6.2f, %+6.2f\n",R2D(hs->roll), R2D(hs->pitch), R2D(hs->yaw)); } if (TESTCONTENT(hs->content,SBS_GYRO)) { fprintf(fp,"Gyro (deg/s): %+6.2f, %+6.2f, %+6.2f\n",R2D(hs->gyro[0]), R2D(hs->gyro[1]), R2D(hs->gyro[2])); } if (TESTCONTENT(hs->content,SBS_ACCEL)) { fprintf(fp,"Accel (m/s2): %+5.2f, %+5.2f, %+5.2f\n",hs->accel[0], hs->accel[1], hs->accel[2]); } if (TESTCONTENT(hs->content,SBS_MAGNETO)) { fprintf(fp,"Magneto: %+5.2f, %+5.2f, %+5.2f\n",hs->magneto[0], hs->magneto[1], hs->magneto[2]); } if (TESTCONTENT(hs->content,SBS_IMUTEMP)) { fprintf(fp,"IMU Temp (degC): %.2f\n",hs->imutemp); } if (TESTCONTENT(hs->content,SBS_ALTITUDE)) { fprintf(fp,"Z Range (m): %+4.2f Filtered (m): %.2f\n", hs->zrange, hs->zfiltered); } if (TESTCONTENT(hs->content,SBS_PRESSURE)) { fprintf(fp,"Pressure (?): %+6.2f\n",hs->pressure); } if (TESTCONTENT(hs->content,SBS_HRANGES)) { int i; fprintf(fp,"H ranges (m): "); for (i=0;i<4;i++) { if (hs->hranges[i]>65.5) { fprintf(fp,"inf "); } else { fprintf(fp,"%4.2f ",hs->hranges[i]); } } printf("\n"); } if (TESTCONTENT(hs->content,SBS_XY_REL)) { fprintf(fp,"XY Rel (m): %.2f, %.2f\n",hs->xrel, hs->yrel); } if (TESTCONTENT(hs->content,SBS_BATTERY)) { fprintf(fp,"Battery (V): %5.2f\n",hs->battery); } if (TESTCONTENT(hs->content,SBS_TIMEOUT)) { fprintf(fp,"Timeout (ms): WD %d CTRL %d \n",hs->watchdogTimeout,hs->controlTimeout); } if (TESTCONTENT(hs->content,SBS_CHANNELS)) { fprintf(fp,"Channels: %.3f %.3f %.3f %.3f\n\t%.3f %.3f %.3f %.3f \n", hs->rcChannel[0],hs->rcChannel[1], hs->rcChannel[2],hs->rcChannel[3], hs->rcChannel[4],hs->rcChannel[5], hs->rcChannel[6],hs->rcChannel[7]); } if (TESTCONTENT(hs->content,SBS_O_ATTITUDE)) { fprintf(fp,"Output Attitude: %f, %f, %f\n",hs->o_attitude[0], hs->o_attitude[1], hs->o_attitude[2]); } if (TESTCONTENT(hs->content,SBS_O_ALTITUDE)) { fprintf(fp,"Output Altitude: %f\n", hs->o_altitude); } if (TESTCONTENT(hs->content,SBS_O_TOL)) { fprintf(fp,"Output TOL: %04X\n",hs->o_tol); } if (TESTCONTENT(hs->content,SBS_O_XY)) { fprintf(fp,"Output XY: %f, %f\n",hs->o_xy[0], hs->o_xy[1]); } if (TESTCONTENT(hs->content,SBS_O_OAVOID)) { fprintf(fp,"Output OA XY: %f, %f\n", hs->o_oavoid[0], hs->o_oavoid[1]); } if (TESTCONTENT(hs->content,SBS_COAXSPEED)) { if (hs->coaxspeed.state & COAXSPEED_AVAILABLE) { if (hs->coaxspeed.state & COAXSPEED_VALID_MEASUREMENT) { fprintf(fp,"CoaxSpeed %02X: V %.3f %.3f L %3d%%\n",hs->coaxspeed.state, hs->coaxspeed.vel_x, hs->coaxspeed.vel_y, hs->coaxspeed.light); } else { fprintf(fp,"CoaxSpeed %02X: V invalid L %3d%%\n",hs->coaxspeed.state, hs->coaxspeed.light); } } else { fprintf(fp,"CoaxSpeed %02X: not detected\n",hs->coaxspeed.state); } } }