int sbSetControlParameters(struct SBControlContext *control,
const struct SBControlParameters *params)
{
int r;
SBSerialisedMessage sm,reply;
SBControlParametersMessage cm;
SBBasicReplyMessage br;
cm.baseThrust = (unsigned short)(1000 * params->baseThrust);
cm.yawOffset = (unsigned short)(1000 * params->yawOffset);
cm.altitudeKp = (signed short)(1000 * params->altitudeKp);
cm.altitudeKi = (signed short)(1000 * params->altitudeKi);
cm.altitudeKd = (signed short)(1000 * params->altitudeKd);
cm.yawKp = (signed short)(1000 * params->yawKp);
cm.yawKi = (signed short)(1000 * params->yawKi);
cm.yawKd = (signed short)(1000 * params->yawKd);
sbCtrlParametersEncode(&sm,control->handle,&cm,1);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbConfigureBluetooth(struct SBControlContext *control,
const char code[4], const char name[16])
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureBluetoothMessage cm;
SBBasicReplyMessage br;
memcpy(cm.code,code,4);
memcpy(cm.name,name,16);
sbConfigureBluetoothEncode(&sm,control->handle,&cm,1);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbSetTrimMode(struct SBControlContext *control, const struct SBTrimMode *mode)
{
int r;
SBSerialisedMessage sm,reply;
SBTrimModeMessage tm;
SBBasicReplyMessage br;
tm.trimMode = mode->trimMode;
tm.rollTrim = (signed short)(10000 * mode->rollTrim);
tm.pitchTrim = (signed short)(10000 * mode->pitchTrim);
sbTrimModeEncode(&sm,control->handle,&tm,1);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// controlMode defined from sbconst.h SB_CTRL_xxxx
int sbConfigureControl(struct SBControlContext *control,
unsigned int roll,
unsigned int pitch,
unsigned int yaw,
unsigned int altitude)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureControl ctrl;
SBBasicReplyMessage br;
ctrl.roll = roll;
ctrl.pitch = pitch;
ctrl.yaw = yaw;
ctrl.altitude = altitude;
sbCfgControlEncode(&sm,control->handle,&ctrl,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbConfigureRawControl(struct SBControlContext *control,
unsigned char speedprofile1, unsigned char speedprofile2)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureRawControl cfg;
SBBasicReplyMessage br;
cfg.speedprofile1 = speedprofile1;
cfg.speedprofile2 = speedprofile2;
sbCfgRawControlEncode(&sm,control->handle,&cfg,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// Only SI unit used here: percent, percent, rad/s, m, m/s
int sbSetControlWithTimestamp(struct SBControlContext *control,
double roll, double pitch, double yaw, double altitude, unsigned long timestamp)
{
int r;
SBSerialisedMessage sm,reply;
SBSetControlWithTimestamp ctrl;
SBBasicReplyMessage br;
ctrl.roll = (signed short)round(roll*1000);
ctrl.pitch = (signed short)round(pitch*1000);
ctrl.yaw = (signed short)round(yaw*1800.0/M_PI);
ctrl.altitude = (signed short)round(altitude*1000.0);
ctrl.timestamp = timestamp;
sbSetControlWithTimestampEncode(&sm,control->handle,&ctrl,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// Use a OR of SBS_... flags to create content
int sbRequestState(struct SBControlContext *control,
unsigned long contents, SBHeliState *state)
{
unsigned int numtry = 0;
SBSerialisedMessage sm;
SBRequestState rs;
rs.content[1] = contents & 0xFFFF;
rs.content[0] = (contents >> 16) & 0xFFFF;
sbReqStateEncode(&sm,control->handle,&rs);
// dumpMessage("sbReqStateEncode",&sm);
do {
numtry += 1;
if (numtry > 5) {
printf("sbRequestState: timeout\n");
return -3;
}
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
printf("sbRequestState: sbSendMessage failed\n");
state->content = 0;
}
if(sbReceiveState(control,state,DEFAULT_TIMEOUT_MS)) {
printf("sbRequestState: sbReceiveState failed\n");
state->content = 0;
}
#ifdef LINUX
usleep(10000);
#endif
#ifdef WIN32
Sleep(10);
#endif
// If the state content is not what we asked for, request it again!
} while (state->content != contents);
return 0;
}
// navMode defined from sbconst.h SB_NAV_xxxx
int sbSetNavMode(struct SBControlContext *control,
unsigned int navMode)
{
int r;
SBSerialisedMessage sm,reply;
SBSetNavigationMode nav;
SBBasicReplyMessage br;
nav.mode = navMode;
sbSetNavEncode(&sm,control->handle,&nav,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
/* Send a reset command to the DSPIC. After this command, the connections may
* not be reusable.
* */
int sbResetDSPIC(struct SBControlContext *control)
{
int r;
SBSerialisedMessage sm,reply;
SBBasicReplyMessage br;
sm.len = 0;
sm.handle = control->handle;
sm.msgid = SB_MSGID_RESET;
if (control->ackMode) {
sm.msgid |= SB_MSGID_REQACK;
}
sbFinalizeMessage(&sm);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbConfigureTimeout(struct SBControlContext *control,
unsigned short control_timeout_ms,
unsigned short watchdog_timeout_ms)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureTimeout timeout;
SBBasicReplyMessage br;
timeout.controlTimeout = control_timeout_ms;
timeout.watchdogTimeout = watchdog_timeout_ms;
sbCfgTimeoutEncode(&sm,control->handle,&timeout,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
/** Control the light of the coax speed, if used as a sensor without control
* */
int sbSetCoaxSpeedLight(struct SBControlContext *control, unsigned char percent)
{
int r;
SBSerialisedMessage sm,reply;
SBCoaxSpeedSetLight setlight;
SBBasicReplyMessage br;
setlight.percent = percent;
sbCoaxSpeedSetLightEncode(&sm,control->handle,&setlight,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbSetRawControl(struct SBControlContext *control,
double motor1, double motor2, double servo1, double servo2)
{
int r;
SBSerialisedMessage sm,reply;
SBRawControl ctrl;
SBBasicReplyMessage br;
ctrl.motor1 = (signed short)(motor1*1000);
ctrl.motor2 = (signed short)(motor2*1000);
ctrl.servo1 = (signed short)(servo1*1000);
ctrl.servo2 = (signed short)(servo2*1000);
sbRawControlEncode(&sm,control->handle,&ctrl,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
} else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// commMode defined from sbconst.h SB_COM_xxxx
int sbConfigureComm(struct SBControlContext *control,
unsigned int commMode,
unsigned int frequency,
unsigned int numMessages,
unsigned long contents)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureCommunication comm;
SBBasicReplyMessage br;
comm.commMode = commMode;
comm.frequency = frequency;
comm.numMessages = numMessages;
comm.content[1] = contents & 0xFFFF;
comm.content[0] = (contents >> 16) & 0xFFFF;
sbCfgCommEncode(&sm,control->handle,&comm,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// oavoidMode defined from sbconst.h SB_OA_xxxx
int sbConfigureOAMode(struct SBControlContext *control, unsigned int oavoidMode)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureObstAvoid oa;
SBBasicReplyMessage br;
oa.oavoidMode = oavoidMode;
sbCfgOAEncode(&sm,control->handle,&oa,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
int sbConfigureCommLoop(struct SBControlContext *control, unsigned int verbose,
int debug_channel)
{
int r;
SBSerialisedMessage sm,reply;
SBConfigureCommLoop cl;
SBBasicReplyMessage br;
cl.verbosity = verbose;
cl.debug_channel = debug_channel;
sbCfgCommLoopEncode(&sm,control->handle,&cl,control->ackMode);
if (control->ackMode) {
if (sendAndWaitMsg(control,&sm,&reply)) {
return -1;
}
r = sbBasicReplyDecode(&reply,&br);
if (r == 0) {
return br.status;
}
}else {
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
return -1;
}
// #define DEBUG_SEND_AND_WAIT
static
int sendAndWaitMsg(struct SBControlContext *control,
SBSerialisedMessage *tosend,
SBSerialisedMessage *reply)
{
unsigned int numtries = 10;
#ifdef DEBUG_SEND_AND_WAIT
printf("sendAndWaitMsg %02X\n",tosend->msgid);
#endif // DEBUG_SEND_AND_WAIT
while (numtries > 0) {
numtries --;
#ifdef DEBUG_SEND_AND_WAIT
printf("Sending message %02X\n",tosend->msgid);
#endif // DEBUG_SEND_AND_WAIT
if(sbSendMessage(&control->channel,tosend,DEFAULT_TIMEOUT_MS)) {
return -1;
}
#ifdef DEBUG_SEND_AND_WAIT
printf("Waiting message %02X\n",tosend->msgid);
#endif // DEBUG_SEND_AND_WAIT
if (sbWaitRawMessage(&control->channel,-1,reply,DEFAULT_TIMEOUT_MS)) {
printf("Timeout %d\n",numtries);
continue;
}
#ifdef DEBUG_SEND_AND_WAIT
printf("Got message %02X\n",reply->msgid);
#endif // DEBUG_SEND_AND_WAIT
if (reply->msgid == (SB_MSGID_REPLY|SB_MSGID_STATE)) {
#ifdef DEBUG_SEND_AND_WAIT
printf("That's a state message\n");
#endif
numtries ++; // that does not count :)
if (control->stateCallback) {
SBHeliStateRaw hr;
SBHeliState hs;
sbStateDecode(reply,&hr);
scaleHeliState(&hs,&hr);
control->stateCallback(&hs,control->userData);
}
} else if (reply->msgid == (SB_MSGID_REPLY|(tosend->msgid&SB_MSGID_MASK))) {
#ifdef DEBUG_SEND_AND_WAIT
printf("Got the ack\n");
#endif // DEBUG_SEND_AND_WAIT
break;
}
}
return numtries?0:-2;
}
/* Send a string to the DSPIC. If the debug interface are active, then this
* string is displayed on the DEBUG console. Otherwise it is ignored.
* The string length cannot be longer than SB_STRING_MESSAGE_LENGTH
* */
int sbSendString(struct SBControlContext *control,const char *text)
{
SBSerialisedMessage sm;
SBStringMessage msg;
#ifdef WIN32
strncpy_s((char*)msg.text,SB_STRING_MESSAGE_LENGTH-1,text,SB_STRING_MESSAGE_LENGTH-1);
#else
strncpy((char*)msg.text,text,SB_STRING_MESSAGE_LENGTH-1);
#endif
msg.text[SB_STRING_MESSAGE_LENGTH-1]=0;
sbStringMsgEncode(&sm,control->handle,&msg);
if(sbSendMessage(&control->channel,&sm,DEFAULT_TIMEOUT_MS)) {
return -1;
}
return 0;
}
// TODO:
// - Remove all access control
// - Add one state agenda function per channel
// - Add a number of message in the communication configuration.
void sbCommLoop(SBCommLoopSpec *comm, SBHeliStateRaw *heliState)
{
static int lastState = -1;
long lastTimecount = -1;
// Initialise loop parameters
comm->stateReady[0] = comm->stateReady[1] = 0;
comm->messageToSend[0] = comm->messageToSend[1] = 0;
comm->cmd_watchdog = 0;
comm->ctrl_watchdog = 0;
comm->timeout_cmd = 5000;
comm->timeout_ctrl = 1000;
comm->timecount = 0;
comm->timeinmode = 0;
// Initialise the global state variable
memset(heliState,0,sizeof(SBHeliStateRaw));
heliState->watchdogTimeout = comm->timeout_cmd;
heliState->controlTimeout = comm->timeout_ctrl;
heliState->mode.navigation = SB_NAV_STOP;
heliState->mode.communication = SB_COM_ONREQUEST;
heliState->mode.oavoid = SB_OA_NONE;
heliState->mode.rollAxis = SB_CTRL_NONE;
heliState->mode.pitchAxis = SB_CTRL_NONE;
heliState->mode.yawAxis = SB_CTRL_NONE;
heliState->mode.altAxis = SB_CTRL_NONE;
heliState->setpoint.rollAxis = SB_CTRL_NONE;
heliState->setpoint.pitchAxis = SB_CTRL_NONE;
heliState->setpoint.yawAxis = SB_CTRL_NONE;
heliState->setpoint.altAxis = SB_CTRL_NONE;
heliState->setpoint.roll = 0;
heliState->setpoint.pitch = 0;
heliState->setpoint.yaw = 0;
heliState->setpoint.altitude = 0;
heliState->control.roll = 0;
heliState->control.pitch = 0;
heliState->control.yaw = 0;
heliState->control.altitude = 0;
heliState->commFrequency = 0;
sbChannelFlush(comm->channel+0);
sbChannelFlush(comm->channel+1);
ERROR(_DEBUG,"Comm loop is started\n");
while (1) {
SBBasicReplyMessage reply;
char text[128];
signed int messageSource;
unsigned int requestAck = 0;
SBSerialisedMessage sm;
SBSerialisedMessage rsm;
#if 1
if (comm->timecount - lastTimecount >= 1000) {
lastTimecount = comm->timecount;
ERROR(_DEBUG,"Is there a message for me?\n");
}
#endif
if (heliState->mode.navigation != lastState) {
sprintf(text,"%s -> %s\n",sbNavModeString(lastState),sbNavModeString(heliState->mode.navigation));
ERROR(_DEBUG,text);
lastState = heliState->mode.navigation;
}
if (comm->debug_channel != CHANNEL_0_ID) {
if (comm->stateReady[CHANNEL_0_ID] && comm->messageToSend[CHANNEL_0_ID]) {
int res;
ERROR(_DEBUG2,"S0");
res = sbSendMessage(comm->channel+CHANNEL_0_ID,&comm->serialisedState,SEND_TIMEOUT);
if (res != 0) {
ERROR(_DEBUG,"*");
sbChannelFlush(comm->channel+CHANNEL_0_ID);
}
comm->messageToSend[CHANNEL_0_ID] -= 1;
comm->stateReady[CHANNEL_0_ID] = 0;
if (comm->messageToSend[CHANNEL_0_ID] == 0) {
comm->setPeriodicState(CHANNEL_0_ID,0);
}
}
}
if (comm->debug_channel != CHANNEL_1_ID) {
if (comm->stateReady[CHANNEL_1_ID] && comm->messageToSend[CHANNEL_1_ID]) {
int res;
ERROR(_DEBUG2,"S1");
res = sbSendMessage(comm->channel+CHANNEL_1_ID,&comm->serialisedState,SEND_TIMEOUT);
if (res != 0) {
ERROR(_DEBUG,"*");
sbChannelFlush(comm->channel+CHANNEL_1_ID);
}
comm->messageToSend[CHANNEL_1_ID] -= 1;
comm->stateReady[CHANNEL_1_ID] = 0;
if (comm->messageToSend[CHANNEL_1_ID] == 0) {
comm->setPeriodicState(CHANNEL_1_ID,0);
}
}
}
messageSource = -1;
if (comm->debug_channel != CHANNEL_0_ID) {
if ((messageSource<0) && !sbChannelWaitData(comm->channel+CHANNEL_0_ID,1)) {
ERROR(_DEBUG2,"There is data UART\n");
if (sbWaitRawMessage(comm->channel+CHANNEL_0_ID,-1, &sm, RECEIVE_TIMEOUT)) {
// this should not fail, but just in case, discard anything
// in the message queue
sbChannelFlush(comm->channel+CHANNEL_0_ID);
ERROR(_WARNING,"Failed to read message\n");
} else {
ERROR(_DEBUG2,"I got the message\n");
messageSource = CHANNEL_0_ID;
}
}
}
if (comm->debug_channel != CHANNEL_1_ID) {
if ((messageSource<0) && !sbChannelWaitData(comm->channel+CHANNEL_1_ID,1)) {
ERROR(_DEBUG2,"There is data on BT\n");
if (sbWaitRawMessage(comm->channel+CHANNEL_1_ID,-1, &sm, RECEIVE_TIMEOUT)) {
// this should not fail, but just in case, discard anything
// in the message queue
sbChannelFlush(comm->channel+CHANNEL_1_ID);
ERROR(_WARNING,"Failed to read message\n");
} else {
ERROR(_DEBUG2,"I got the message\n");
messageSource = CHANNEL_1_ID;
}
}
}
if (messageSource<0) {
continue;
#if 1
} else {
//sprintf(text,"Received message %02X (%d) from %02X\n\r",sm.msgid,sm.msgid,messageSource);
sprintf(text,"[%02X]",sm.msgid);
ERROR(_DEBUG,text);
#endif
}
#if 1
requestAck = (sm.msgid & SB_MSGID_REQACK);
sm.msgid = sm.msgid & SB_MSGID_MASK;
reply.status = SB_REPLY_OK;
switch (sm.msgid) {
/*** Function allowed from any source ***/
case SB_MSGID_CFG_COMMLOOP:
{
SBConfigureCommLoop col;
if (sbCfgCommLoopDecode(&sm,&col)) {
ERROR(_ERROR,"CFG_COMMLOOP: sbCfgCommLoopDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
#if 0
if (col.debug_channel & 0xFE) {
comm->debug_channel = 0xFF;
comm->verbose = 0; // no debug channel, no need to bother with sprinf
} else {
comm->debug_channel = col.debug_channel;
comm->verbose = col.verbosity;
}
#else
// Channel selection is ignored for now
comm->verbose = col.verbosity;
#endif
reply.status = SB_REPLY_OK;
}
if (comm->verbose) {
ERROR(_DEBUG,"Set CommLoop Verbosity\n");
} else {
ERROR(_WARNING,"Stopped CommLoop Verbosity\n");
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_COMMLOOP: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_COMMLOOP: sbSendMessage");
}
}
break;
case SB_MSGID_STRING:
{
SBStringMessage s;
if (sbStringMsgDecode(&sm,&s)) {
// Ignore...?
} else {
// just in case
s.text[SB_STRING_MESSAGE_LENGTH-1] = 0;
ERROR(_DEBUG,(const char*)s.text);
}
}
break;
case SB_MSGID_KEEP_ALIVE:
{
comm->cmd_watchdog = 0;
if (requestAck) {
reply.status = SB_REPLY_OK;
if (sbBasicReplyEncode(&rsm,0,
sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"KEEP_ALIVE: sbBasicReplyEncode");
} else if(sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"KEEP_ALIVE: sbSendMessage");
}
}
ERROR(_DEBUG,"Keep Alive\n");
}
break;
case SB_MSGID_CFG_COMM:
{
SBConfigureCommunication com;
if (sbCfgCommDecode(&sm,&com)) {
ERROR(_ERROR,"CFG_COMM: sbCfgCommDecode");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->mode.communication = com.commMode & 0x01;
if (heliState->mode.communication == SB_COM_CONTINUOUS) {
unsigned int period;
heliState->content[0] = com.content[0] & comm->capacities[0];
heliState->content[1] = com.content[1] & comm->capacities[1];
if (com.frequency == 0) com.frequency = 1;
heliState->commFrequency = com.frequency;
period = 1000/com.frequency;
comm->messageToSend[messageSource] = com.numMessages;
comm->setPeriodicState(messageSource,period);
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Started continuous communication\n");
} else {
comm->messageToSend[messageSource] = 0;
heliState->commFrequency = 0;
comm->setPeriodicState(messageSource,0);
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Started on-demand communication\n");
}
}
if (sbBasicReplyEncode(&rsm,0,
sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_COMM: sbBasicReplyEncode");
} else if(sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_COMM: sbSendMessage");
}
}
break;
case SB_MSGID_STATE:
{
SBRequestState rs;
if (sbReqStateDecode(&sm,&rs)) {
ERROR(_ERROR,"STATE: sbReqStateDecode");
} else {
SBHeliStateRaw localState;
rs.content[0] = rs.content[0] & comm->capacities[0];
rs.content[1] = rs.content[1] & comm->capacities[1];
comm->updateHeliState();
memcpy(&localState,heliState,sizeof(SBHeliStateRaw));
localState.content[0] = rs.content[0];
localState.content[1] = rs.content[1];
// ERROR(_DEBUG,"Encoding request\n");
if (sbStateEncode(&rsm,0,&localState)) {
ERROR(_ERROR,"STATE: Failed to encode heli state\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"STATE: sbSendMessage");
}
sprintf(text,"Accepted state request %04X %04X\n",
rs.content[0],rs.content[1]);
ERROR(_DEBUG,text);
}
}
break;
case SB_MSGID_GET_VERSION:
{
const SBVersionStatus *version = sbGetCompiledVersion();
if (sbVersionMsgEncode(&rsm,0,version)) {
ERROR(_ERROR,"GET_VERSION: sbVersionMsgEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"GET_VERSION: sbSendMessage");
}
ERROR(_DEBUG,"Received version request\n");
}
break;
case SB_MSGID_SENSORLIST:
{
SBSensorListMessage sl;
sl.content[0] = comm->capacities[0];
sl.content[1] = comm->capacities[1];
if (sbSensorListEncode(&rsm,0,&sl)) {
ERROR(_ERROR,"SENSORLIST: sbSensorListEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SENSORLIST: sbSendMessage");
}
ERROR(_DEBUG,"Received sensor list request\n");
}
break;
/*** Connection function ***/
case SB_MSGID_CONNECT:
{
reply.status = SB_REPLY_OK;
comm->cmd_watchdog = 0;
ERROR(_DEBUG,"Connection accepted\n");
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CONNECT: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CONNECT: sbSendMessage");
}
}
break;
/*** Functions allowed only from the active Controller ***/
case SB_MSGID_DISCONNECT:
{
reply.status = SB_REPLY_OK;
if (sbBasicReplyEncode(&rsm,0,
sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"DISCONNECT: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"DISCONNECT: sbSendMessage");
}
ERROR(_DEBUG,"Disconnection accepted\n");
}
break;
case SB_MSGID_RESET:
{
reply.status = SB_REPLY_OK;
if (sbBasicReplyEncode(&rsm,0,
sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"RESET: sbBasicReplyEncode\n");
} else {
sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT);
// TODO: error management
}
ERROR(_DEBUG,"RESET Received\n");
#if 1
if (comm->reset) comm->reset();
while (1) {};
#endif
}
break;
case SB_MSGID_CFG_BLUETOOTH:
{
SBConfigureBluetoothMessage cm;
if (sbConfigureBluetoothDecode(&sm,&cm)) {
ERROR(_ERROR,"CFG_BLUETOOTH: sbConfigureBluetoothDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else if (comm->configureBluetooth) {
int res = comm->configureBluetooth(cm.code,cm.name);
reply.status = (res==0)?SB_REPLY_OK:SB_REPLY_ERROR;
} else {
reply.status = SB_REPLY_OK;
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_BLUETOOTH: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_BLUETOOTH: sbSendMessage");
}
ERROR(_DEBUG,"Configured Bluetooth (suggest reset)\n");
}
break;
case SB_MSGID_SET_LIGHT:
{
SBCoaxSpeedSetLight sl;
if (sbCoaxSpeedSetLightDecode(&sm,&sl)) {
ERROR(_ERROR,"SET_LIGHT: sbCoaxSpeedSetLightDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
if (comm->setLight) {
comm->setLight(sl.percent);
}
reply.status = SB_REPLY_OK;
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"SET_LIGHT: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SET_LIGHT: sbSendMessage");
}
ERROR(_DEBUG,"Set CoaxSpeed Light\n");
}
break;
case SB_MSGID_CFG_OAVOID:
{
SBConfigureObstAvoid coa;
if (sbCfgOADecode(&sm,&coa)) {
ERROR(_ERROR,"CFG_OAVOID: sbCfgOADecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->mode.oavoid = coa.oavoidMode & 0x03;
reply.status = SB_REPLY_OK;
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_OAVOID: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_OAVOID: sbSendMessage");
}
ERROR(_DEBUG,"Configured Obstacle Avoidance\n");
}
break;
case SB_MSGID_SET_DEBUG:
{
SBSetDebugMode cdbg;
if (sbSetDebugDecode(&sm,&cdbg)) {
ERROR(_ERROR,"SET_DEBUG: sbSetDebugDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
comm->debugMode = cdbg.debugMode;
reply.status = SB_REPLY_OK;
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"SET_DEBUG: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SET_DEBUG: sbSendMessage");
}
ERROR(_DEBUG,"Configured Debug Mode\n");
}
break;
case SB_MSGID_REPEAT:
{
unsigned int messageDest = -1;
// printf("Repeat message\n");
reply.status = SB_REPLY_OK;
switch (messageSource) {
case CHANNEL_0_ID:
if (comm->debug_channel != CHANNEL_1_ID) {
messageDest = CHANNEL_1_ID;
}
break;
case CHANNEL_1_ID:
if (comm->debug_channel != CHANNEL_0_ID) {
messageDest = CHANNEL_0_ID;
}
break;
}
if (messageDest != -1) {
// printf("Sending repeat from %d to %d\n",messageSource,messageDest);
if (sbSendMessage(comm->channel+messageDest,&sm,SEND_TIMEOUT)) {
ERROR(_ERROR,"REPEAT: repeated sbSendMessage");
reply.status = SB_REPLY_ERROR;
}
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"REPEAT: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"REPEAT: sbSendMessage");
}
ERROR(_DEBUG,"Repeated message\n");
}
break;
case SB_MSGID_CFG_TIMEOUT:
{
SBConfigureTimeout cot;
if (sbCfgTimeoutDecode(&sm,&cot)) {
ERROR(_ERROR,"CFG_TIMEOUT: sbCfgTimeoutDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
// a timeout of zero is ignored
if (cot.controlTimeout) {
comm->timeout_ctrl = cot.controlTimeout;
}
if (cot.watchdogTimeout) {
comm->timeout_cmd = cot.watchdogTimeout;
}
heliState->watchdogTimeout = comm->timeout_cmd;
heliState->controlTimeout = comm->timeout_ctrl;
reply.status = SB_REPLY_OK;
sprintf(text,"Configured Timeout: %d & %d\n",
heliState->controlTimeout,heliState->watchdogTimeout);
ERROR(_DEBUG,text);
comm->cmd_watchdog = 0;
comm->ctrl_watchdog = 0;
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_TIMEOUT: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_TIMEOUT: sbSendMessage");
}
}
break;
case SB_MSGID_TRIMMODE:
{
SBTrimModeMessage msg;
if (!comm->updateTrimMode) {
ERROR(_WARNING,"Ignored Trim Mode (No handler)\n");
break;
}
if (sm.len == 0) {
// Get message
comm->updateTrimMode(0,&msg);
if (sbTrimModeEncode(&rsm,0,&msg,0)) {
ERROR(_ERROR,"TRIM_MODE: sbTrimModeEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"TRIM_MODE: sbSendMessage");
}
} else {
// Set message
if (sbTrimModeDecode(&sm,&msg)) {
ERROR(_ERROR,"TRIM_MODE: sbTrimModeDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else if (comm->updateTrimMode(1,&msg)) {
reply.status = SB_REPLY_ERROR;
ERROR(_WARNING,"Ignored Trim Mode\n");
} else {
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Configured Trim Mode\n");
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"TRIM_MODE: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"TRIM_MODE: sbSendMessage");
}
}
}
break;
case SB_MSGID_CUSTOM:
{
SBCustomMessage msg;
if (sm.len == 0) {
// Get message
if (sbCustomMsgEncode(&rsm,0,&comm->customState,1)) {
ERROR(_ERROR,"CUSTOM: sbCustomMsgEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CUSTOM: sbSendMessage");
}
} else {
// Set message
if (sbCustomMsgDecode(&sm,&msg)) {
ERROR(_ERROR,"CUSTOM: sbCustomMsgDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
memcpy(&comm->customState,&msg,sizeof(SBCustomMessage));
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"CUSTOM: Set custom message\n");
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CUSTOM: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CUSTOM: sbSendMessage");
}
}
}
break;
case SB_MSGID_CTRLPARM:
{
SBControlParametersMessage msg;
if (!comm->updateControlParams) {
ERROR(_WARNING,"Ignored Control Parameters (No handler)\n");
break;
}
if (sm.len == 0) {
// Get message
comm->updateControlParams(0,&msg);
if (sbCtrlParametersEncode(&rsm,0,&msg,0)) {
ERROR(_ERROR,"CTRL_PARM: sbTrimModeEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CTRL_PARM: sbSendMessage");
}
} else {
// Set message
if (sbCtrlParametersDecode(&sm,&msg)) {
ERROR(_ERROR,"CTRL_PARM: sbCtrlParametersDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else if (comm->updateControlParams(1,&msg)) {
reply.status = SB_REPLY_ERROR;
ERROR(_WARNING,"Ignored Control Parameters\n");
} else {
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Configured Control Parameters\n");
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CTRL_PARM: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CTRL_PARM: sbSendMessage");
}
}
}
break;
case SB_MSGID_CFG_RAW_CMD:
{
SBConfigureRawControl cot;
if (sbCfgRawControlDecode(&sm,&cot)) {
ERROR(_ERROR,"CFG_RAW_CMD: sbCfgRawControlDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->rawSpeedProfile[0] = cot.speedprofile1;
heliState->rawSpeedProfile[1] = cot.speedprofile2;
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Configured Raw Command\n");
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_RAW_CMD: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_RAW_CMD: sbSendMessage");
}
}
break;
case SB_MSGID_CFG_CONTROL:
{
SBConfigureControl cot;
if (sbCfgControlDecode(&sm,&cot)) {
ERROR(_ERROR,"CFG_CONTROL: sbCfgControlDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->setpoint.rollAxis = cot.roll;
heliState->setpoint.pitchAxis = cot.pitch;
heliState->setpoint.yawAxis = cot.yaw;
heliState->setpoint.altAxis = cot.altitude;
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Configured Control Command\n");
#if 0
printf("%s %s %s %s\n",
sbCtrlModeString(heliState->setpoint.rollAxis),
sbCtrlModeString(heliState->setpoint.pitchAxis),
sbCtrlModeString(heliState->setpoint.yawAxis),
sbCtrlModeString(heliState->setpoint.altAxis));
#endif
}
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"CFG_CONTROL: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"CFG_CONTROL: sbSendMessage");
}
}
break;
case SB_MSGID_SET_CONTROL:
{
SBSetControl sc;
// printf("%8ld New Control Command\n",comm->timecount);
if (heliState->mode.navigation == SB_NAV_CTRLLED) {
if (sbSetControlDecode(&sm,&sc)) {
ERROR(_ERROR,"SET_CONTROL: sbSetControlDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->setpoint.roll = sc.roll;
heliState->setpoint.pitch = sc.pitch;
heliState->setpoint.yaw = sc.yaw;
heliState->setpoint.altitude = sc.altitude;
comm->ctrl_watchdog = 0;
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Accepted Control Command\n");
}
} else {
reply.status = SB_REPLY_INVALID_NAVMODE;
}
comm->cmd_watchdog = 0;
if (requestAck) {
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"SET_CONTROL: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SET_CONTROL: sbSendMessage");
}
} else {
// TODO?
}
}
break;
case SB_MSGID_SET_CONTROL_WITH_TIMESTAMP:
{
SBSetControlWithTimestamp sc;
if (heliState->mode.navigation == SB_NAV_CTRLLED) {
if (sbSetControlWithTimestampDecode(&sm,&sc)) {
ERROR(_ERROR,"SET_CONTROL: sbSetControlWithTimestampDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->setpoint.roll = sc.roll;
heliState->setpoint.pitch = sc.pitch;
heliState->setpoint.yaw = sc.yaw;
heliState->setpoint.altitude = sc.altitude;
comm->ctrl_watchdog = 0;
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Accepted Control Command\n");
}
} else {
reply.status = SB_REPLY_INVALID_NAVMODE;
}
comm->cmd_watchdog = 0;
if (requestAck) {
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"SET_CONTROL: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SET_CONTROL: sbSendMessage");
}
} else {
// TODO?
}
}
break;
case SB_MSGID_RAW_COMMAND:
{
SBRawControl sc;
if (heliState->mode.navigation == SB_NAV_RAW) {
if (sbRawControlDecode(&sm,&sc)) {
ERROR(_ERROR,"RAW_COMMAND: sbRawControlDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else {
heliState->setpoint.motor1 = sc.motor1;
heliState->setpoint.motor2 = sc.motor2;
heliState->setpoint.servo1 = sc.servo1;
heliState->setpoint.servo2 = sc.servo2;
comm->ctrl_watchdog = 0;
reply.status = SB_REPLY_OK;
ERROR(_DEBUG,"Accepted Raw Command\n");
}
} else {
reply.status = SB_REPLY_INVALID_NAVMODE;
}
comm->cmd_watchdog = 0;
if (requestAck) {
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"RAW_COMMAND: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"RAW_COMMAND: sbSendMessage");
}
} else {
// TODO?
}
}
break;
case SB_MSGID_SET_NAVMODE:
{
SBSetNavigationMode nm;
unsigned char navMode = heliState->mode.navigation;
reply.status = SB_REPLY_OK;
if (sbSetNavDecode(&sm,&nm)) {
ERROR(_ERROR,"SET_NAVMODE: sbSetNavDecode\n");
reply.status = SB_REPLY_DECODE_FAILURE;
} else if (nm.mode != navMode) {
switch (nm.mode) {
case SB_NAV_STOP:
if ((navMode != SB_NAV_IDLE) && (navMode != SB_NAV_RAW)) {
reply.status = SB_REPLY_INVALID_NAVMODE;
}
break;
case SB_NAV_IDLE:
if (navMode != SB_NAV_STOP) reply.status = SB_REPLY_INVALID_NAVMODE;
break;
case SB_NAV_RAW:
if (navMode != SB_NAV_STOP) reply.status = SB_REPLY_INVALID_NAVMODE;
heliState->setpoint.motor1 = 0;
heliState->setpoint.motor2 = 0;
heliState->setpoint.servo1 = 0;
heliState->setpoint.servo2 = 0;
comm->ctrl_watchdog = 0;
break;
case SB_NAV_TAKEOFF:
switch (navMode) {
case SB_NAV_IDLE:
case SB_NAV_HOVER:
break;
default:
reply.status = SB_REPLY_INVALID_NAVMODE;
}
if (comm->timeinmode < MIN_TIME_IN_IDLE) reply.status = SB_REPLY_TOO_EARLY;
break;
case SB_NAV_LAND:
switch (navMode) {
case SB_NAV_IDLE:
case SB_NAV_HOVER:
case SB_NAV_TAKEOFF:
case SB_NAV_RAW:
case SB_NAV_SINK:
case SB_NAV_CTRLLED:
break;
default:
reply.status = SB_REPLY_INVALID_NAVMODE;
}
break;
case SB_NAV_HOVER:
if ((navMode != SB_NAV_SINK) && (navMode != SB_NAV_CTRLLED)) {
reply.status = SB_REPLY_INVALID_NAVMODE;
}
break;
case SB_NAV_CTRLLED:
if ((navMode != SB_NAV_SINK) && (navMode != SB_NAV_HOVER)) {
reply.status = SB_REPLY_INVALID_NAVMODE;
} else {
// Sensible values when switching modes
heliState->setpoint.roll = 0;
heliState->setpoint.pitch = 0;
heliState->setpoint.yaw = 0;
heliState->setpoint.altitude = 0;
if (heliState->setpoint.yawAxis == SB_CTRL_POS) {
heliState->setpoint.yaw = heliState->yaw;
}
if (heliState->setpoint.altAxis == SB_CTRL_REL) {
heliState->setpoint.altitude = heliState->zrange;
}
comm->ctrl_watchdog = 0;
}
break;
case SB_NAV_SINK:
default :
// this is not a mode one can ask
reply.status = SB_REPLY_INVALID_NAVMODE;
break;
}
} else {
// no change requested
reply.status = SB_REPLY_OK;
}
comm->cmd_watchdog = 0;
if (requestAck) {
if (sbBasicReplyEncode(&rsm,0, sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"SET_NAVMODE: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,SEND_TIMEOUT)) {
ERROR(_ERROR,"SET_NAVMODE: sbSendMessage");
}
} else {
// TODO?
}
if (reply.status == SB_REPLY_OK) {
heliState->mode.navigation = nm.mode;
sprintf(text,"Accepted Nav Mode %s\n",sbNavModeString(nm.mode));
ERROR(_DEBUG,text);
} else {
sprintf(text,"Ignored Nav Mode %s\n",sbNavModeString(nm.mode));
ERROR(_DEBUG,text);
}
}
break;
default:
{
ERROR(_ERROR,"Unknown command\n");
reply.status = SB_REPLY_UNKNOWN;
if (sbBasicReplyEncode(&rsm,0,
sm.msgid|SB_MSGID_REPLY,&reply)) {
ERROR(_ERROR,"Unknown command: sbBasicReplyEncode\n");
} else if (sbSendMessage(comm->channel+messageSource,&rsm,500)) {
ERROR(_ERROR,"SB_XXXX: sbSendMessage");
}
sprintf(text,"\n\rUnhandled message <%02X>\n",sm.msgid);
ERROR(_DEBUG,text);
}
break;
}
if (reply.status != SB_REPLY_OK) {
sprintf(text,"Msg %02X: Error code %d\n",sm.msgid,reply.status);
ERROR(_WARNING,text);
} else {
sprintf(text,"Msg %02X: success\n",sm.msgid);
ERROR(_DEBUG,text);
}
#endif
}
}
