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