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