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