/*
* Performs control operation on a tty device.
*/
PRIVATE int tty_ioctl(unsigned minor, unsigned cmd, unsigned arg)
{
int ret;
UNUSED(minor);
/* Parse command. */
switch (cmd)
{
/* Get tty settings. */
case TTY_GETS:
ret = tty_gets(&tty, (struct termios *)arg);
break;
/* Clear console. */
case TTY_CLEAR:
ret = tty_clear(&tty);
break;
/* Invalid operation. */
default:
ret = -EINVAL;
break;
}
return (ret);
}
static int modem_stat(char *cmd, slist_t *oks, slist_t *ers,int timeout, char *stat, size_t stat_len)
{
char buf[MAX_STRING];int rc;
slist_t *cs;time_t t1=t_set(timeout);
DEBUG(('M',4,"modem_stat: cmd=\"%s\" timeout=%d",cmd,timeout));
rc=modem_sendstr(cmd);
if(rc!=1) {
if(stat) xstrcpy(stat, "FAILURE", stat_len);
DEBUG(('M',3,"modem_stat: modem_sendstr failed, rc=%d",rc));
return MC_FAIL;
}
if(!oks && !ers) return MC_OK;
rc=OK;
while(ISTO(rc) && !t_exp(t1)) {
rc=tty_gets(buf, MAX_STRING-1, t_rest(t1));
if(!*buf) continue;
if(rc!=OK) {
if(stat) xstrcat(stat, "FAILURE",
stat_len);
DEBUG(('M',3,"modem_stat: tty_gets failed"));
return MC_FAIL;
}
for(cs=oks;cs;cs=cs->next)
if(!strncmp(buf, cs->str, strlen(cs->str))) {
if(stat) xstrcat(stat, buf,
stat_len);
return MC_OK;
}
for(cs=ers;cs;cs=cs->next)
if(!strncmp(buf, cs->str, strlen(cs->str))) {
if(stat) xstrcat(stat, buf,
stat_len);
return MC_ERROR;
}
if(stat)
{
xstrcat(stat,buf,stat_len);
xstrcat(stat,"\n",stat_len);
}
}
if(stat) {
if (ISTO(rc)) xstrcat(stat, "TIMEOUT", stat_len);
else xstrcat(stat, "FAILURE", stat_len);
}
return MC_FAIL;
}
int modem_chat(char *cmd, slist_t *oks, slist_t *nds, slist_t *ers, slist_t *bys,
char *ringing, int maxr, int timeout, char *rest, size_t restlen)
{
char buf[MAX_STRING];
int rc,nrng=0;
slist_t *cs;
time_t t1;
calling=1;
DEBUG(('M',4,"modem_chat: cmd=\"%s\" timeout=%d",cmd,timeout));
rc=modem_sendstr(cmd);
if(rc!=1) {
if(rest) xstrcpy(rest, "FAILURE", restlen);
DEBUG(('M',3,"modem_chat: modem_sendstr failed, rc=%d",rc));
return MC_FAIL;
}
if(!oks && !ers && !bys) return MC_OK;
rc=OK;
t1=t_set(timeout);
while(ISTO(rc) && !t_exp(t1) && (!maxr || nrng<maxr)) {
getevt();
rc=tty_gets(buf, MAX_STRING-1, t_rest(t1));
if(rc==RCDO) {
if(rest)xstrcpy(rest,"HANGUP",restlen);
return MC_BUSY;
}
if(rc!=OK) {
if(rest) xstrcpy(rest, "FAILURE", restlen);
DEBUG(('M',3,"modem_chat: tty_gets failed, rc=%d",rc));
return MC_FAIL;
}
if(!*buf)continue;
for(cs=oks;cs;cs=cs->next)
if(!strncmp(buf,cs->str,strlen(cs->str))) {
if(rest)xstrcpy(rest,buf,restlen);
return MC_OK;
}
for(cs=ers;cs;cs=cs->next)
if(!strncmp(buf,cs->str,strlen(cs->str))) {
if(rest)xstrcpy(rest,buf,restlen);
return MC_ERROR;
}
if(ringing&&!strncmp(buf,ringing,strlen(ringing))) {
if(!nrng&&strlen(ringing)==4) {
if(rest)xstrcpy(rest,buf,restlen);
return MC_RING;
}
nrng++;
continue;
}
for(cs=nds;cs;cs=cs->next)
if(!strncmp(buf,cs->str,strlen(cs->str))) {
if(rest)xstrcpy(rest,buf,restlen);
return MC_NODIAL;
}
for(cs=bys;cs;cs=cs->next)
if(!strncmp(buf,cs->str,strlen(cs->str))) {
if(rest)xstrcpy(rest,buf,restlen);
return MC_BUSY;
}
}
if(rest) {
if(nrng && maxr && nrng>=maxr) snprintf(rest, restlen, "%d RINGINGs", nrng);
else if(ISTO(rc)) xstrcpy(rest, "TIMEOUT", restlen);
else xstrcpy(rest, "FAILURE", restlen);
}
return MC_FAIL;
}
void do_dds_shell (DDS_DataWriter dw)
{
Vector3_t m;
DDS_InstanceHandle_t h;
char buf [256];
#if !defined (NUTTX_RTOS)
tty_init ();
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
#endif
#if 0
printf ("Welcome to the Technicolor chatroom.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
#endif
h = 0;
while (!aborting) {
#if defined (NUTTX_RTOS)
/* Take into account that fgets reads the "\n" character at the end
of each line. Code should consider this aspect in every case */
fgets(buf, 256, stdin);
#else
tty_gets (sizeof (buf), buf, 0, 1);
#endif
if (buf [0] == '!') {
#if defined (NUTTX_RTOS)
if (!strcmp (buf + 1, "quit\n") ||
(buf [1] == 'q' && buf [2] == '\n')) {
#else
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
#endif
aborting = 1;
break;
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "list\n"))
#else
else if (!strcmp (buf + 1, "list"))
#endif
printf ("Attendees:\r\n\t%s\r\n", user_name);
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "user\n", 4)) {
#else
else if (!memcmp (buf + 1, "user", 4)) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "room\n", 4)) {
#else
else if (!memcmp (buf + 1, "room", 4)) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
strcpy (chatroom, buf + 6);
printf ("Switched to chatroom: %s\r\n", chatroom);
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "info\n"))
#else
else if (!strcmp (buf + 1, "info"))
#endif
printf ("Chatroom: %s, Username: %s\r\n",
chatroom, user_name);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "busy\n"))
#else
else if (!strcmp (buf + 1, "busy"))
#endif
Vector3_signal (dw, h, 0);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "away\n")) {
#else
else if (!strcmp (buf + 1, "away")) {
#endif
if (h) {
Vector3_signal (dw, h, 1);
h = 0;
}
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "help\n") ||
(buf [1] == 'h' && buf [2] == '\n\0') ||
(buf [1] == '?' && buf [2] == '\n\0')) {
#else
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
#endif
printf ("Commands:\r\n");
printf (" !room <room_name> -> set the chatroom name.\r\n");
printf (" !user <user_name> -> set the user name.\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !info -> show chatroom and user.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
} else {
printf("Writing to the other endpoints not allowed.\n");
#if 0
if (!h)
h = ChatMsg_register (dw, &m);
//sprintf(buf_t, "Embedded says %s\n", buf);
fetch_imu();
//m.message = buf_t;
m.message = buf;
ChatMsg_write (dw, &m, h);
#endif
printf ("Commands:\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
}
}
}
static void *dds_send_imu (void *args)
{
Vector3_t m;
DDS_InstanceHandle_t h;
h = 0;
for (;;){
sleep (1); // sleep 0.5 seconds
//sprintf(buf_t, "Embedded says %s\n", buf);
m.x_ = 1;
m.y_ = 2;
m.z_ = 3;
#if 0
/* According to https://github.com/brunodebus/tinq-core/issues/7#issuecomment-63740498:
the Vector3 shouldn't be registered if it doesn't contain a @key attribute
*/
if (!h)
h = Vector3_register (dw, &m);
#endif
Vector3_write (dw, &m, h);
}
}
void read_msg (DDS_DataReaderListener *l, DDS_DataReader dr)
{
Vector3_t msg;
DDS_InstanceStateKind kind;
int valid;
DDS_ReturnCode_t ret;
ARG_NOT_USED (l)
memset (&msg, 0, sizeof (msg));
ret = Vector3_read_or_take (dr, &msg, DDS_NOT_READ_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE, 1,
&valid, &kind);
if (ret == DDS_RETCODE_OK)
do {
#if 0
/* Use a from field within the message to avoid to read the messages that this
node sent.
*/
#ifndef DISPLAY_SELF
if (!strcmp (msg.from, user_name) &&
!strcmp (msg.chatroom, chatroom))
break;
#endif
#endif
if (valid)
printf ("IMU accel message: x=%f, y=%f and z=%f\r\n", msg.x_, msg.y_, msg.z_);
else if (kind == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE!\r\n");
else if (kind == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE!\r\n");
}
while (0);
Vector3_cleanup (&msg);
}
#ifdef WAITSETS
static void *imu_reader (void *args)
{
DDS_DynamicDataReader dr;
DDS_WaitSet ws;
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_ReadCondition rc;
DDS_ConditionSeq conds = DDS_SEQ_INITIALIZER (DDS_Condition);
DDS_Duration_t to;
DDS_ReturnCode_t ret;
dr = args;
ws = DDS_WaitSet__alloc ();
if (!ws)
fatal ("Unable to allocate a WaitSet!");
if (verbose)
printf ("DDS Waitset allocated.\r\n");
rc = DDS_DataReader_create_readcondition (dr, ss, vs, is);
if (!rc)
fatal ("DDS_DataReader_create_readcondition () returned an error!");
if (verbose)
printf ("DDS Readcondition created.\r\n");
ret = DDS_WaitSet_attach_condition (ws, rc);
if (ret)
fatal ("Unable to attach condition to a WaitSet!");
while (!aborting) {
to.sec = 0;
to.nanosec = 200000000; /* Timeout after 200ms. */
ret = DDS_WaitSet_wait (ws, &conds, &to);
if (ret == DDS_RETCODE_TIMEOUT)
continue;
read_msg (NULL, dr);
}
ret = DDS_WaitSet_detach_condition (ws, rc);
if (ret)
fatal ("Unable to detach condition from WaitSet (%s)!", DDS_error (ret));
DDS_WaitSet__free (ws);
return (NULL);
}
static void start_imu_reader (DDS_DynamicDataReader dr)
{
thread_create (rt, imu_reader, dr);
}
void do_dds_shell (DDS_DataWriter dw)
{
Vector3_dos_t m;
DDS_InstanceHandle_t h;
char buf [256];
tty_init ();
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
#if 0
printf ("Welcome to the Technicolor chatroom.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
#endif
h = 0;
while (!aborting) {
tty_gets (sizeof (buf), buf, 0, 1);
if (buf [0] == '!') {
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
aborting = 1;
break;
}
else if (!strcmp (buf + 1, "list"))
printf ("Attendees:\r\n\t%s\r\n", user_name);
else if (!memcmp (buf + 1, "user", 4)) {
if (h) {
Vector3_dos_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
else if (!strcmp (buf + 1, "busy"))
Vector3_dos_signal (dw, h, 0);
else if (!strcmp (buf + 1, "away")) {
if (h) {
Vector3_dos_signal (dw, h, 1);
h = 0;
}
}
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
printf ("Commands:\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
} else {
#if 0
if (!h)
h = Vector3_dos_register (dw, &m);
m.message = buf;
Vector3_dos_write (dw, &m, h);
#endif
printf ("Commands:\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
}
}
}
void do_chat (DDS_DataWriter dw)
{
ChatMsg_t m;
DDS_InstanceHandle_t h;
char buf [256];
#if !defined (NUTTX_RTOS)
tty_init ();
#endif
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
printf ("Welcome to the ROS 2.0 DDS chatroom.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf (" (Please write messages of less than 255 characters).\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
h = 0;
while (!aborting) {
#if defined (NUTTX_RTOS)
/* Take into account that fgets reads the "\n" character at the end
of each line. Code should consider this aspect in every case */
fgets(buf, 256, stdin);
#else
tty_gets (sizeof (buf), buf, 0, 1);
#endif
if (buf [0] == '!') {
#if defined (NUTTX_RTOS)
if (!strcmp (buf + 1, "quit\n") ||
(buf [1] == 'q' && buf [2] == '\n')) {
#else
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
#endif
aborting = 1;
break;
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "list\n"))
#else
else if (!strcmp (buf + 1, "list"))
#endif
printf ("Attendees:\r\n\t%s\r\n", user_name);
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "user\n", 4)) {
#else
else if (!memcmp (buf + 1, "user", 4)) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "room\n", 4)) {
#else
else if (!memcmp (buf + 1, "room", 4)) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
strcpy (chatroom, buf + 6);
printf ("Switched to chatroom: %s\r\n", chatroom);
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "info\n"))
#else
else if (!strcmp (buf + 1, "info"))
#endif
printf ("Chatroom: %s, Username: %s\r\n",
chatroom, user_name);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "busy\n"))
#else
else if (!strcmp (buf + 1, "busy"))
#endif
ChatMsg_signal (dw, h, 0);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "away\n")) {
#else
else if (!strcmp (buf + 1, "away")) {
#endif
if (h) {
ChatMsg_signal (dw, h, 1);
h = 0;
}
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "help\n") ||
(buf [1] == 'h' && buf [2] == '\n\0') ||
(buf [1] == '?' && buf [2] == '\n\0')) {
#else
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
#endif
printf ("Commands:\r\n");
printf (" !room <room_name> -> set the chatroom name.\r\n");
printf (" !user <user_name> -> set the user name.\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !info -> show chatroom and user.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
}
if (!h)
h = ChatMsg_register (dw, &m);
m.message = buf;
ChatMsg_write (dw, &m, h);
}
}
void read_msg (DDS_DataReaderListener *l, DDS_DataReader dr)
{
ChatMsg_t msg;
DDS_InstanceStateKind kind;
int valid;
DDS_ReturnCode_t ret;
ARG_NOT_USED (l)
memset (&msg, 0, sizeof (msg));
ret = ChatMsg_read_or_take (dr, &msg, DDS_NOT_READ_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE, 1,
&valid, &kind);
if (ret == DDS_RETCODE_OK)
do {
#ifndef DISPLAY_SELF
if (!strcmp (msg.from, user_name) &&
!strcmp (msg.chatroom, chatroom))
break;
#endif
if (valid)
printf ("%s: %s\r\n", msg.from, msg.message);
else if (kind == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
printf ("%s is busy!\r\n", msg.from);
else if (kind == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
printf ("%s has left!\r\n", msg.from);
}
while (0);
ChatMsg_cleanup (&msg);
}
#ifdef WAITSETS
static void *chat_reader (void *args)
{
DDS_DynamicDataReader dr;
DDS_WaitSet ws;
DDS_SampleStateMask ss = DDS_NOT_READ_SAMPLE_STATE;
DDS_ViewStateMask vs = DDS_ANY_VIEW_STATE;
DDS_InstanceStateMask is = DDS_ANY_INSTANCE_STATE;
DDS_ReadCondition rc;
DDS_ConditionSeq conds = DDS_SEQ_INITIALIZER (DDS_Condition);
DDS_Duration_t to;
DDS_ReturnCode_t ret;
dr = args;
ws = DDS_WaitSet__alloc ();
if (!ws)
fatal ("Unable to allocate a WaitSet!");
if (verbose)
printf ("DDS Waitset allocated.\r\n");
rc = DDS_DataReader_create_readcondition (dr, ss, vs, is);
if (!rc)
fatal ("DDS_DataReader_create_readcondition () returned an error!");
if (verbose)
printf ("DDS Readcondition created.\r\n");
ret = DDS_WaitSet_attach_condition (ws, rc);
if (ret)
fatal ("Unable to attach condition to a WaitSet!");
while (!aborting) {
to.sec = 0;
to.nanosec = 200000000; /* Timeout after 200ms. */
ret = DDS_WaitSet_wait (ws, &conds, &to);
if (ret == DDS_RETCODE_TIMEOUT)
continue;
read_msg (NULL, dr);
}
ret = DDS_WaitSet_detach_condition (ws, rc);
if (ret)
fatal ("Unable to detach condition from WaitSet (%s)!", DDS_error (ret));
DDS_WaitSet__free (ws);
return (NULL);
}
static void start_chat_reader (DDS_DynamicDataReader dr)
{
thread_create (rt, chat_reader, dr);
}
static void stop_chat_reader (DDS_DynamicDataReader dr)
{
ARG_NOT_USED (dr)
thread_wait (rt, NULL);
}
#else
static DDS_DataReaderListener msg_listener = {
NULL, /* Sample rejected. */
NULL, /* Liveliness changed. */
NULL, /* Requested Deadline missed. */
NULL, /* Requested incompatible QoS. */
read_msg, /* Data available. */
NULL, /* Subscription matched. */
NULL, /* Sample lost. */
NULL /* Cookie */
};
#endif
#ifdef DDS_SECURITY
#define fail_unless assert
static void enable_security (void)
{
DDS_Credentials credentials;
DDS_ReturnCode_t error;
#ifdef MSECPLUG_WITH_SECXML
/*int dhandle, thandle;*/
#endif
error = DDS_SP_set_policy ();
if (error)
fatal ("DDS_SP_set_policy() returned error (%s)!", DDS_error (error));
#ifdef MSECPLUG_WITH_SECXML
if (DDS_SP_parse_xml ("security.xml"))
fatal ("SP: no DDS security rules in 'security.xml'!\r\n");
#else
DDS_SP_add_domain();
if (!realm_name)
DDS_SP_add_participant ();
else
DDS_SP_set_participant_access (DDS_SP_add_participant (), strcat(realm_name, "*"), 2, 0);
#endif
if (!cert_path || !key_path)
fatal ("Error: you must provide a valid certificate path and a valid private key path\r\n");
if (engine_id) {
DDS_SP_init_engine (engine_id, init_engine_fs);
credentials.credentialKind = DDS_ENGINE_BASED;
credentials.info.engine.engine_id = engine_id;
credentials.info.engine.cert_id = cert_path;
credentials.info.engine.priv_key_id = key_path;
}
else {
credentials.credentialKind = DDS_FILE_BASED;
credentials.info.filenames.private_key_file = key_path;
credentials.info.filenames.certificate_chain_file = cert_path;
}
error = DDS_Security_set_credentials ("Technicolor Chatroom", &credentials);
}
void dds_dump (DDS_DomainParticipant part)
{
char buf [256], cmd [64];
const char *sp;
ARG_NOT_USED (part)
LIST_INIT (topics);
LIST_INIT (endpoints);
tty_init ();
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
printf ("Technicolor DDS Dump program.\r\n");
printf ("Type 'help' for a list ofcommands.\r\n");
while (!aborting) {
printf (">");
fflush (stdout);
tty_gets (sizeof (buf), buf, 0, 0);
sp = buf;
skip_blanks (&sp);
skip_string (&sp, cmd);
if (!memcmp (cmd, "help", 1)) {
printf ("Following commands are available:\r\n");
printf ("\tlist List all discovered domain topics.\r\n");
printf ("\tauto <name_spec> Automatically monitor new matching topics.\r\n");
printf ("\tmonitor <name_spec> Start monitoring matching topic(s).\r\n");
printf ("\tignore <name_spec> Stop monitoring matching topic(s).\r\n");
printf ("\ttrace Toggle tracing of discovery traffic.\r\n");
#ifndef DDS_DEBUG
printf ("\tpause Pause monitoring.\r\n");
printf ("\tresume Resume monitoring.\r\n");
printf ("\tquit Quit the program.\r\n");
#else
DDS_Debug_help ();
#endif
}
else if (!memcmp (cmd, "list", 2))
t_dump ();
else if (!memcmp (cmd, "auto", 2))
t_auto (sp);
else if (!memcmp (cmd, "monitor", 2))
t_monitor (sp, part);
else if (!memcmp (cmd, "ignore", 2))
t_ignore (sp, part);
else if (!memcmp (cmd, "trace", 2))
t_trace ();
#ifndef DDS_DEBUG
else if (!memcmp (cmd, "pause", 1))
paused = 1;
else if (!memcmp (cmd, "resume", 1))
paused = 0;
else if (!memcmp (cmd, "quit", 1))
aborting = 1;
else
printf ("?%s\r\n", buf);
#else
else
DDS_Debug_command (buf);
#endif
}
void do_dds (DDS_DataWriter dw)
{
Imu_t m;
DDS_InstanceHandle_t h;
char buf [256];
#if !defined (NUTTX_RTOS)
tty_init ();
DDS_Handle_attach (tty_stdin,
POLLIN | POLLPRI | POLLERR | POLLHUP | POLLNVAL,
tty_input,
NULL);
#endif
#if 0
printf ("ROS 2 Embedded DDSIMU test program.\r\n");
printf ("Anything you type will be sent to all chatroom attendees.\r\n");
printf (" (Please write messages of less than 255 characters).\r\n");
printf ("Type '!help' for chatroom options.\r\n");
m.chatroom = chatroom;
m.from = user_name;
#endif
h = 0;
while (!aborting) {
#if defined (NUTTX_RTOS)
/* Take into account that fgets reads the "\n" character at the end
of each line. Code should consider this aspect in every case */
fgets(buf, 256, stdin);
#else
tty_gets (sizeof (buf), buf, 0, 1);
#endif
if (buf [0] == '!') {
#if defined (NUTTX_RTOS)
if (!strcmp (buf + 1, "quit\n") ||
(buf [1] == 'q' && buf [2] == '\n')) {
#else
if (!strcmp (buf + 1, "quit") ||
(buf [1] == 'q' && buf [2] == '\0')) {
#endif
aborting = 1;
break;
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "list\n"))
#else
else if (!strcmp (buf + 1, "list"))
#endif
printf ("Attendees:\r\n\t%s\r\n", user_name);
#if defined (NUTTX_RTOS)
else if (!memcmp (buf + 1, "user\n", 4)) {
#else
else if (!memcmp (buf + 1, "user", 4)) {
#endif
if (h) {
Imu_signal (dw, h, 1);
h = 0;
}
strcpy (user_name, buf + 6);
printf ("You are now: %s\r\n", user_name);
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "busy\n"))
#else
else if (!strcmp (buf + 1, "busy"))
#endif
Imu_signal (dw, h, 0);
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "away\n")) {
#else
else if (!strcmp (buf + 1, "away")) {
#endif
if (h) {
Imu_signal (dw, h, 1);
h = 0;
}
}
#if defined (NUTTX_RTOS)
else if (!strcmp (buf + 1, "help\n") ||
(buf [1] == 'h' && buf [2] == '\n\0') ||
(buf [1] == '?' && buf [2] == '\n\0')) {
#else
else if (!strcmp (buf + 1, "help") ||
(buf [1] == 'h' && buf [2] == '\0') ||
(buf [1] == '?' && buf [2] == '\0')) {
#endif
printf ("Commands:\r\n");
printf (" !list -> list the attendees.\r\n");
printf (" !busy -> momentarily not involved.\r\n");
printf (" !away -> gone away.\r\n");
printf (" !help or !h or !? -> Show this info.\r\n");
printf (" !quit or !q -> Quit the chatroom.\r\n");
printf (" !!<command> -> DDS debug command.\r\n");
/* printf (" !$<command> -> Shell command.\r\n"); */
}
else if (buf [1] == '!')
DDS_Debug_command (buf + 2);
/* else if (buf [1] == '$')
system (buf + 2); */
else
printf ("?%s\r\n", buf + 1);
continue;
} else {
printf("Writing to the other endpoints not allowed. Type !!help for DDS Shell commands.\n");
#if 0
if (!h)
h = Imu_register (dw, &m);
//sprintf(buf_t, "Embedded says %s\n", buf);
fetch_imu();
//m.message = buf_t;
m.message = buf;
Imu_write (dw, &m, h);
#endif
}
}
}
#define G_CONST 9.8 // 1 g = 9.8 m/s^2
//char buf_t [256];
float accx;
float accy;
float accz;
void fetch_imu(void)
{
// units m/s^2
accx =G_CONST*read_accel_x()/1000;
accy =G_CONST*read_accel_y()/1000;
accz =G_CONST*read_accel_z()/1000;
// printf("acc_x: %d, acc_y: %d, acc_z: %d\n", accx, accy, accz);
// sprintf(buf_t, "Embedded says hi\n");
}
// print out the speed of publishing
#define TIMETEST
static void *dds_send_imu (void *args)
{
Imu_t m;
DDS_InstanceHandle_t h;
// Init I2C and print registers
setup_i2c();
print_config_i2c();
h = 0;
#ifdef TIMETEST
int nruns = 0;
uint32_t start;
uint32_t elapsed;
start = clock_systimer();
#endif
for (;;){
//sleep (0.5);
fetch_imu();
#if 0
m.orientation.x = 10;
m.orientation.y = 10;
m.orientation.z = 10;
m.orientation.w = 10;
m.orientation_covariance[0] = 0;
m.orientation_covariance[1] = 1;
m.orientation_covariance[2] = 2;
m.orientation_covariance[3] = 3;
m.orientation_covariance[4] = 4;
m.orientation_covariance[5] = 5;
m.orientation_covariance[6] = 6;
m.orientation_covariance[7] = 7;
m.orientation_covariance[8] = 8;
#endif
m.linear_acceleration.x_ = accx;
m.linear_acceleration.y_ = accy;
m.linear_acceleration.z_ = accz;
#if 0
/* According to https://github.com/brunodebus/tinq-core/issues/7#issuecomment-63740498:
the Imu shouldn't be registered if it doesn't contain a @key attribute
*/
if (!h)
h = Imu_register (dw, &m);
#endif
Imu_write (dw, &m, h);
#ifdef TIMETEST
nruns++;
if (nruns == 10){
elapsed = clock_systimer() - start; // in miliseconds
float period = (10.0*1000) / (float) elapsed;
printf("elapsed: %d miliseconds to send 10 readings\n", elapsed);
//printf("Period: %f Hz\n", period);
start = clock_systimer();
nruns = 0;
}
#endif
}
}
void read_msg (DDS_DataReaderListener *l, DDS_DataReader dr)
{
Imu_t msg;
DDS_InstanceStateKind kind;
int valid;
DDS_ReturnCode_t ret;
ARG_NOT_USED (l)
memset (&msg, 0, sizeof (msg));
ret = Imu_read_or_take (dr, &msg, DDS_NOT_READ_SAMPLE_STATE,
DDS_ANY_VIEW_STATE,
DDS_ANY_INSTANCE_STATE, 1,
&valid, &kind);
if (ret == DDS_RETCODE_OK)
do {
#if 0
#ifndef DISPLAY_SELF
if (!strcmp (msg.from, user_name) &&
!strcmp (msg.chatroom, chatroom))
break;
#endif
#endif
if (valid){
#if 0
printf ("Imu ROS message received:\n");
printf(" header.seq=%d\n", msg.header.seq);
printf(" header.stamp.sec=%d\n", msg.header.stamp.sec);
printf(" header.stamp.nanosec=%d\n", msg.header.stamp.nanosec);
printf(" header.frame_id=%d\n", msg.header.frame_id);
printf(" orientation.x=%f\n", msg.orientation.x);
printf(" orientation.y=%f\n", msg.orientation.y);
printf(" orientation.z=%f\n", msg.orientation.z);
printf(" orientation.w=%f\n", msg.orientation.w);
printf(" orientation_covariance=%f %f %f %f %f %f %f %f %f\n", msg.orientation_covariance[0],
msg.orientation_covariance[1], msg.orientation_covariance[2], msg.orientation_covariance[3],
msg.orientation_covariance[4], msg.orientation_covariance[5], msg.orientation_covariance[6],
msg.orientation_covariance[7], msg.orientation_covariance[8]);
printf(" angular_velocity_x=%f\n", msg.angular_velocity.x_);
printf(" angular_velocity_y=%f\n", msg.angular_velocity.y_);
printf(" angular_velocity_z=%f\n", msg.angular_velocity.z_);
printf(" angular_velocity_covariance=%f %f %f %f %f %f %f %f %f\n", msg.angular_velocity_covariance[0],
msg.angular_velocity_covariance[1], msg.angular_velocity_covariance[2], msg.angular_velocity_covariance[3],
msg.angular_velocity_covariance[4], msg.angular_velocity_covariance[5], msg.angular_velocity_covariance[6],
msg.angular_velocity_covariance[7], msg.angular_velocity_covariance[8]);
printf(" linear_acceleration_x=%f\n", msg.linear_acceleration.x_);
printf(" linear_acceleration_y=%f\n", msg.linear_acceleration.y_);
printf(" linear_acceleration_z=%f\n", msg.linear_acceleration.z_);
printf(" linear_acceleration_covariance=%f %f %f %f %f %f %f %f %f\n", msg.linear_acceleration_covariance[0],
msg.linear_acceleration_covariance[1], msg.linear_acceleration_covariance[2], msg.linear_acceleration_covariance[3],
msg.linear_acceleration_covariance[4], msg.linear_acceleration_covariance[5], msg.linear_acceleration_covariance[6],
msg.linear_acceleration_covariance[7], msg.linear_acceleration_covariance[8]);
#endif
}
else if (kind == DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_DISPOSED_INSTANCE_STATE!\r\n");
else if (kind == DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE)
printf ("DDS_NOT_ALIVE_NO_WRITERS_INSTANCE_STATE!\r\n");
}
while (0);
Imu_cleanup (&msg);
}
char *
sal_console_gets(char *buf, int size)
{
return tty_gets(buf, size);
}
