// 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
}
}
// Flush the communication channel
int sbFlushCommunication(struct SBControlContext *control)
{
return sbChannelFlush(&control->channel);
}