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