int main(int argc, char **argv)
{
int res;
const SBVersionStatus * compiled = sbGetCompiledVersion();
unsigned int objSizeStatus = sbValidateObjectSize(compiled);
ros::init(argc, argv, "coax_repeat");
ROS_INFO("Object size status: %04x",objSizeStatus);
assert(objSizeStatus == 0);
SBApiSimpleContext simple;
SBController api(&simple);
CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1])));
ros::NodeHandle n("/coax_repeat");
#ifdef REPEAT_STRINGS
ros::Subscriber repeat_sub = n.subscribe("torepeat",1,&SBController::repeat_ros_callback,&api);
ros::Publisher repeat_pub = n.advertise<std_msgs::String>("repeated",1);
#else
ros::Subscriber repeat_sub = n.subscribe("torepeat",1,&SBController::repeat_ros_callback,&api);
ros::Publisher repeat_pub = n.advertise<coax_msgs::CoaxRepeat>("repeated",1);
#endif
api.setupRepeatIO(&repeat_pub,&repeat_sub);
ROS_INFO("Coax Server ready");
ros::spin();
DEBUG(res = api.terminate());
return 0;
}
int main(int argc, const char *argv[])
{
int res;
const SBVersionStatus * compiled = sbGetCompiledVersion();
unsigned int objSizeStatus = sbValidateObjectSize(compiled);
printf("Object size status: %04x\n",objSizeStatus);
assert(objSizeStatus == 0);
SBApiSimpleContext simple;
SBController api(&simple);
if (argc < 4) {
printf("Usage:\n\t%s <port> <load|save> <filename>\n",argv[0]);
return -1;
}
CRITICAL(res = api.initialise(argv[1]));
if (strcasecmp(argv[2],"load")==0) {
api.load(argv[3]);
}
if (strcasecmp(argv[2],"save")==0) {
api.save(argv[3]);
}
DEBUG(res = api.terminate());
return 0;
}
int main(int argc, const char *argv[])
{
const SBVersionStatus * compiled = sbGetCompiledVersion();
unsigned int objSizeStatus = sbValidateObjectSize(compiled);
printf("Object size status: %04x\n",objSizeStatus);
assert(objSizeStatus == 0);
char flogname[128];
sprintf(flogname,"log-%08ld.txt",(unsigned long)time(NULL));
SBApiSimpleContext simple;
SBController api(&simple);
int res;
CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1])));
DEBUG(res = api.joyctrl());
DEBUG(res = api.terminate());
printf("Saving log\n");
api.savelog(flogname);
printf("Saved log\n");
return 0;
}
int main(int argc, char **argv)
{
int res;
const SBVersionStatus * compiled = sbGetCompiledVersion();
unsigned int objSizeStatus = sbValidateObjectSize(compiled);
ros::init(argc, argv, "coax_server");
ROS_INFO("Object size status: %04x",objSizeStatus);
assert(objSizeStatus == 0);
SBApiSimpleContext simple;
SBController api(&simple);
CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1])));
ros::NodeHandle n("/coax_server");
std::vector<ros::ServiceServer> services;
services.push_back(n.advertiseService("get_version", &SBController::get_version, &api));
services.push_back(n.advertiseService("configure_comm", &SBController::configure_comm, &api));
services.push_back(n.advertiseService("configure_control", &SBController::configure_control, &api));
services.push_back(n.advertiseService("configure_oamode", &SBController::configure_oamode, &api));
services.push_back(n.advertiseService("get_control_parameters", &SBController::get_control_parameters, &api));
services.push_back(n.advertiseService("set_control_parameters", &SBController::set_control_parameters, &api));
services.push_back(n.advertiseService("get_trim_mode", &SBController::get_trim_mode, &api));
services.push_back(n.advertiseService("set_trim_mode", &SBController::set_trim_mode, &api));
services.push_back(n.advertiseService("get_sensor_list", &SBController::get_sensor_list, &api));
services.push_back(n.advertiseService("reach_nav_state", &SBController::reach_nav_state, &api));
services.push_back(n.advertiseService("request_state", &SBController::request_state, &api));
services.push_back(n.advertiseService("send_string", &SBController::send_string, &api));
services.push_back(n.advertiseService("set_verbose", &SBController::set_verbose, &api));
services.push_back(n.advertiseService("set_ack_mode", &SBController::set_ack_mode, &api));
services.push_back(n.advertiseService("set_timeout", &SBController::set_timeout, &api));
services.push_back(n.advertiseService("set_control", &SBController::set_control, &api));
services.push_back(n.advertiseService("set_raw_control", &SBController::set_raw_control, &api));
services.push_back(n.advertiseService("reset", &SBController::reset, &api));
services.push_back(n.advertiseService("set_light", &SBController::set_light, &api));
// Publishing the state
ros::Publisher coax_pub = n.advertise<coax_msgs::CoaxState>("state",50);
api.registerPublisher(&coax_pub);
// Subscribing to control message (without acknowledgement)
ros::TransportHints hints;
ros::Subscriber control_sub = n.subscribe("control",10,&SBController::control_callback,&api,hints.udp());
ros::Subscriber rawcontrol_sub = n.subscribe("rawcontrol",10,&SBController::raw_control_callback,&api,hints.udp());
ROS_INFO("Coax Server ready");
ros::spin();
DEBUG(res = api.terminate());
return 0;
}
int main(int argc, const char *argv[])
{
int res;
const SBVersionStatus * compiled = sbGetCompiledVersion();
unsigned int objSizeStatus = sbValidateObjectSize(compiled);
printf("Object size status: %04x\n",objSizeStatus);
assert(objSizeStatus == 0);
SBApiSimpleContext simple;
SBController api(&simple);
if (argc < 3) {
printf("Usage:\n\t%s <port> <debug>\n",argv[0]);
return -1;
}
int debug = 0;
sscanf(argv[2]," %d",&debug);
CRITICAL(res = api.initialise(argv[1],debug));
DEBUG(res = api.terminate());
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;
}
// 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
}
}
