int sbSetNavAndWait(struct SBControlContext *control, int setmode, int waitmode,double timout_sec) { SBHeliState state; int res; double t,t0 = now(); sbLockCommunication(control); DEBUG(res = sbSetNavMode(control,setmode)); sbUnlockCommunication(control); if (res) return res; while (1) { sbLockCommunication(control); DEBUG(res = sbRequestState(control, SBS_MODES, &state)); sbUnlockCommunication(control); if (res) return res; if (state.mode.navigation == waitmode){ break; } t = now(); if (t < t0) t += 86400; // time of day wrap around if ((t - t0) > timout_sec) { break; } #ifdef LINUX usleep(5000); #endif #ifdef WIN32 Sleep(5); #endif } return (state.mode.navigation == waitmode)?0:+1; }
bool request_state(coax_msgs::CoaxRequestState::Request &req, coax_msgs::CoaxRequestState::Response &out ) { SBHeliState state; sbLockCommunication(&simple->control); DEBUG(res = sbRequestState(&simple->control,req.contents & sensorList,&state)); sbUnlockCommunication(&simple->control); ROS_INFO("Request state [%d]", res); #define COPY(V) out.state.V = state.V out.state.header.stamp = ros::Time::now(); out.state.header.seq = 0; out.state.header.frame_id = "request"; COPY(errorFlags); COPY(content); COPY(timeStamp); COPY(controlTimeout); COPY(watchdogTimeout); COPY(mode.navigation); COPY(mode.communication); COPY(mode.oavoid); COPY(mode.rollAxis); COPY(mode.pitchAxis); COPY(mode.yawAxis); COPY(mode.altAxis); COPY(roll); COPY(pitch); COPY(yaw); for (unsigned int i=0;i<3;i++) { COPY(gyro[i]); COPY(accel[i]); COPY(magneto[i]); COPY(o_attitude[i]); } COPY(imutemp); COPY(zrange); COPY(zfiltered); COPY(pressure); COPY(battery); for (unsigned int i=0;i<4;i++) { COPY(hranges[i]); } for (unsigned int i=0;i<8;i++) { COPY(rcChannel[i]); } COPY(o_altitude); COPY(o_tol); for (unsigned int i=0;i<2;i++) { COPY(o_oavoid[i]); COPY(o_xy[i]); } #undef COPY return true; }
int sbSimpleReachNavState(SBApiSimpleContext *context, int desired, double timeout_sec) { int res; SBHeliState state; double t0 = now(); double t_lastprint = -2; static int transition[8*8] = { // to: stop idle takeoff land hover cntrl sink raw SB_NAV_STOP, SB_NAV_IDLE, SB_NAV_IDLE, -1, SB_NAV_IDLE, SB_NAV_IDLE, -1, SB_NAV_RAW,// from stop SB_NAV_STOP, SB_NAV_IDLE, SB_NAV_TAKEOFF, -1, SB_NAV_TAKEOFF, SB_NAV_TAKEOFF, -1, SB_NAV_STOP, // from idle SB_NAV_LAND, SB_NAV_LAND, SB_NAV_TAKEOFF, SB_NAV_LAND, SB_NAV_TAKEOFF, SB_NAV_TAKEOFF, -1, SB_NAV_LAND,// from takeoff SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, -1, SB_NAV_LAND,// from land SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_HOVER, SB_NAV_CTRLLED, -1, SB_NAV_LAND,// from hover SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_HOVER, SB_NAV_CTRLLED, -1, SB_NAV_LAND,// from ctrlled SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_HOVER, SB_NAV_CTRLLED, -1, SB_NAV_LAND,// from sink SB_NAV_STOP, SB_NAV_STOP, -1, SB_NAV_LAND, SB_NAV_LAND, SB_NAV_LAND, -1, SB_NAV_RAW // from raw (no transition allowed) }; switch (desired) { case SB_NAV_TAKEOFF: case SB_NAV_LAND: printf("sbSimpleReachNavState: Take-off and landing mode are transcient and cannot be used as target for sbSimpleReachNavState\n"); return -1; case SB_NAV_SINK: printf("sbSimpleReachNavState: Sink mode is an error mode and cannot be used as target for sbSimpleReachNavState\n"); return -1; default: break; } switch (context->commMode) { case SB_COM_CONTINUOUS: DEBUG(res = sbSimpleWaitState(context,&state,1.0)); break; case SB_COM_ONREQUEST: sbLockCommunication(&context->control); DEBUG(res = sbRequestState(&context->control,SBS_MODES,&state)); sbUnlockCommunication(&context->control); break; default: res = -1; } if (res) { printf("sbSimpleReachNavState: Could not receive initial state\n"); return -1; } if (!(state.content & SBS_MODES)) { printf("sbSimpleReachNavState: initial state did not contains MODES\n"); return -1; } if (desired == state.mode.navigation) { return 0; } if ((desired != SB_NAV_STOP) && (state.errorFlags)) { printf("sbSimpleReachNavState: refusing to switch mode while error flags are raised\n"); sbErrorPrint(stdout,state.errorFlags); return -1; } while (state.mode.navigation != desired) { double t = now(); int nextstate; switch (context->commMode) { case SB_COM_CONTINUOUS: DEBUG(res = sbSimpleWaitState(context,&state,1.0)); break; case SB_COM_ONREQUEST: sbLockCommunication(&context->control); DEBUG(res = sbRequestState(&context->control,SBS_MODES,&state)); sbUnlockCommunication(&context->control); break; default: res = -1; } if (res) { printf("sbSimpleReachNavState: failed to receive state\n"); return -4; } nextstate = transition[state.mode.navigation*8+desired]; // Testing the validity of the transition if (nextstate < 0) { printf("sbSimpleReachNavState: cannot reach desired state. Aborting\n"); return -2; } // printf("sbSimpleReachNavState: next state = %d / current %d\n",nextstate, state.mode.navigation); if (t < t0) t += 86400; if (t > t0 + timeout_sec) { printf("sbSimpleReachNavState timed out. Aborting\n"); return -2; } if (t - t_lastprint > 2) { printf("Current %s desired: %s next %s wait %s rem t %.3f\n", sbNavModeString(state.mode.navigation), sbNavModeString(desired),sbNavModeString(nextstate), sbNavModeString(waitmode(nextstate)),t0 + timeout_sec - t); t_lastprint = t; } // Requesting the transition sbLockCommunication(&context->control); res = sbSetNavMode(&context->control,nextstate); res = sbKeepAlive(&context->control); sbUnlockCommunication(&context->control); switch (res) { case 0: // Good, it works break; case SB_REPLY_TOO_EARLY: continue; break; default: printf("sbSetNavAndWait failed (%d) to set desired mode. Aborting\n",res); } #ifdef LINUX usleep(100000); #endif #ifdef WIN32 Sleep(100); #endif } // At this stage, the mode is the desired mode. if (state.mode.navigation == SB_NAV_CTRLLED) { // Confirm the control mode when we reach the // controlled state sbLockCommunication(&context->control); res = sbConfigureControl(&context->control, context->rollCtrlMode, context->pitchCtrlMode, context->yawCtrlMode, context->altCtrlMode); sbUnlockCommunication(&context->control); } return 0; }
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; }