Kazufumi Honda / Mbed 2 deprecated RosSerialModule

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

mbedserial.cpp@0:4289fbe66d45, 2020-06-21 (annotated)

Committer:
Kize
Date:
Sun Jun 21 01:48:47 2020 +0000
Revision:
0:4289fbe66d45
new commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kize 0:4289fbe66d45 1 // mbedserial.cpp
Kize 0:4289fbe66d45 2 // serial communication : mbed <-> ROS
Kize 0:4289fbe66d45 3
Kize 0:4289fbe66d45 4 #include "mbedserial.h"
Kize 0:4289fbe66d45 5
Kize 0:4289fbe66d45 6 /* init */
Kize 0:4289fbe66d45 7 void _nullfunc(){ ; };
Kize 0:4289fbe66d45 8
Kize 0:4289fbe66d45 9 Mbedserial::Mbedserial(Serial &pc) : rospc(pc)
Kize 0:4289fbe66d45 10 {
Kize 0:4289fbe66d45 11 msg_buf = new char[256];
Kize 0:4289fbe66d45 12 bufsize = 0;
Kize 0:4289fbe66d45 13 endmsg = '\n';
Kize 0:4289fbe66d45 14 floatarraysize = 0;
Kize 0:4289fbe66d45 15 intarraysize = 0;
Kize 0:4289fbe66d45 16 chararraysize = 0;
Kize 0:4289fbe66d45 17 for(int i=0;i<3;i++){
Kize 0:4289fbe66d45 18 pfunccb[i] = _nullfunc;
Kize 0:4289fbe66d45 19 }
Kize 0:4289fbe66d45 20 rospc.attach(callback(this,&Mbedserial::rcv_callback),Serial::RxIrq);
Kize 0:4289fbe66d45 21 }
Kize 0:4289fbe66d45 22
Kize 0:4289fbe66d45 23 /* receive */
Kize 0:4289fbe66d45 24 void Mbedserial::rcv_callback()
Kize 0:4289fbe66d45 25 {
Kize 0:4289fbe66d45 26 msg_buf[bufsize] = rospc.getc();
Kize 0:4289fbe66d45 27 if(msg_buf[bufsize] == endmsg){
Kize 0:4289fbe66d45 28 switch(msg_buf[0]){
Kize 0:4289fbe66d45 29 case 'f':
Kize 0:4289fbe66d45 30 floatarraysize = *(int *)(&msg_buf[1]);
Kize 0:4289fbe66d45 31 //memcpy(&floatarraysize,&msg_buf[1],4);
Kize 0:4289fbe66d45 32 if(bufsize == floatarraysize * 4 + 5){
Kize 0:4289fbe66d45 33 for(int i=0;i<floatarraysize;i++){
Kize 0:4289fbe66d45 34 getfloat[i] = *(float *)(&msg_buf[i*4+5]);
Kize 0:4289fbe66d45 35 //memcpy(&getfloat[i],&msg_buf[i*4+5]);
Kize 0:4289fbe66d45 36 }
Kize 0:4289fbe66d45 37 pfunccb[0]();
Kize 0:4289fbe66d45 38 }
Kize 0:4289fbe66d45 39 break;
Kize 0:4289fbe66d45 40 case 'i':
Kize 0:4289fbe66d45 41 intarraysize = *(int *)(&msg_buf[1]);
Kize 0:4289fbe66d45 42 //memcpy(&intarraysize, &msg_buf[1], 4);
Kize 0:4289fbe66d45 43 if (bufsize == intarraysize * 4 + 5)
Kize 0:4289fbe66d45 44 {
Kize 0:4289fbe66d45 45 for (int i = 0; i < intarraysize; i++)
Kize 0:4289fbe66d45 46 {
Kize 0:4289fbe66d45 47 getint[i] = *(int *)(&msg_buf[i * 4 + 5]);
Kize 0:4289fbe66d45 48 //memcpy(&getint[i], &msg_buf[i * 4 + 5], 4);
Kize 0:4289fbe66d45 49 }
Kize 0:4289fbe66d45 50 pfunccb[1]();
Kize 0:4289fbe66d45 51 }
Kize 0:4289fbe66d45 52 break;
Kize 0:4289fbe66d45 53 case 'c':
Kize 0:4289fbe66d45 54 chararraysize = *(int *)(&msg_buf[1]);
Kize 0:4289fbe66d45 55 //memcpy(&chararraysize, &msg_buf[1], 4);
Kize 0:4289fbe66d45 56 if (bufsize == chararraysize + 5)
Kize 0:4289fbe66d45 57 {
Kize 0:4289fbe66d45 58 memcpy(&getchar[0], &msg_buf[5], chararraysize);
Kize 0:4289fbe66d45 59 pfunccb[2]();
Kize 0:4289fbe66d45 60 }
Kize 0:4289fbe66d45 61 break;
Kize 0:4289fbe66d45 62 }
Kize 0:4289fbe66d45 63 delete[] msg_buf;
Kize 0:4289fbe66d45 64 bufsize = 0;
Kize 0:4289fbe66d45 65 msg_buf = new char[256];
Kize 0:4289fbe66d45 66 }else{
Kize 0:4289fbe66d45 67 bufsize++;
Kize 0:4289fbe66d45 68 }
Kize 0:4289fbe66d45 69 }
Kize 0:4289fbe66d45 70
Kize 0:4289fbe66d45 71 /* send */
Kize 0:4289fbe66d45 72 int pub_wait_time = 8; //ms
Kize 0:4289fbe66d45 73
Kize 0:4289fbe66d45 74 void Mbedserial::float_write(float array[], int arraysize)
Kize 0:4289fbe66d45 75 {
Kize 0:4289fbe66d45 76 // send data type
Kize 0:4289fbe66d45 77 char msg_type = 'f';
Kize 0:4289fbe66d45 78 rospc.putc(msg_type);
Kize 0:4289fbe66d45 79
Kize 0:4289fbe66d45 80 // send array size
Kize 0:4289fbe66d45 81 char arraysize_c[4];
Kize 0:4289fbe66d45 82 *(int *)arraysize_c = arraysize;
Kize 0:4289fbe66d45 83 //memcpy(arraysize_c, &arraysize, 4);
Kize 0:4289fbe66d45 84 for (int i = 0; i < 4; i++)
Kize 0:4289fbe66d45 85 {
Kize 0:4289fbe66d45 86 rospc.putc(arraysize_c[i]);
Kize 0:4289fbe66d45 87 }
Kize 0:4289fbe66d45 88
Kize 0:4289fbe66d45 89 // send float data
Kize 0:4289fbe66d45 90 char array_c[4];
Kize 0:4289fbe66d45 91 for (int i = 0; i < arraysize; i++)
Kize 0:4289fbe66d45 92 {
Kize 0:4289fbe66d45 93 *(float *)array_c = array[i];
Kize 0:4289fbe66d45 94 //memcpy(array_c, &array[i], 4);
Kize 0:4289fbe66d45 95 for (int j = 0; j < 4; j++)
Kize 0:4289fbe66d45 96 {
Kize 0:4289fbe66d45 97 rospc.putc(array_c[j]);
Kize 0:4289fbe66d45 98 }
Kize 0:4289fbe66d45 99 }
Kize 0:4289fbe66d45 100
Kize 0:4289fbe66d45 101 // send end message
Kize 0:4289fbe66d45 102 rospc.putc(endmsg);
Kize 0:4289fbe66d45 103 //wait_ms(pub_wait_time);
Kize 0:4289fbe66d45 104 }
Kize 0:4289fbe66d45 105
Kize 0:4289fbe66d45 106 void Mbedserial::int_write(int *array, int arraysize)
Kize 0:4289fbe66d45 107 {
Kize 0:4289fbe66d45 108 // send data type
Kize 0:4289fbe66d45 109 char msg_type = 'i';
Kize 0:4289fbe66d45 110 rospc.putc(msg_type);
Kize 0:4289fbe66d45 111
Kize 0:4289fbe66d45 112 // send array size
Kize 0:4289fbe66d45 113 char arraysize_c[4];
Kize 0:4289fbe66d45 114
Kize 0:4289fbe66d45 115 *(int *)arraysize_c = arraysize;
Kize 0:4289fbe66d45 116 //memcpy(arraysize_c, &arraysize, 4);
Kize 0:4289fbe66d45 117 for (int i = 0; i < 4; i++)
Kize 0:4289fbe66d45 118 {
Kize 0:4289fbe66d45 119 rospc.putc(arraysize_c[i]);
Kize 0:4289fbe66d45 120 }
Kize 0:4289fbe66d45 121
Kize 0:4289fbe66d45 122 // send int data
Kize 0:4289fbe66d45 123 char array_c[4];
Kize 0:4289fbe66d45 124 for (int i = 0; i < arraysize; i++)
Kize 0:4289fbe66d45 125 {
Kize 0:4289fbe66d45 126 *(int *)array_c = array[i];
Kize 0:4289fbe66d45 127 //memcpy(array_c, &array[i], 4);
Kize 0:4289fbe66d45 128 for (int j = 0; j < 4; j++)
Kize 0:4289fbe66d45 129 {
Kize 0:4289fbe66d45 130 rospc.putc(array_c[j]);
Kize 0:4289fbe66d45 131 }
Kize 0:4289fbe66d45 132 }
Kize 0:4289fbe66d45 133
Kize 0:4289fbe66d45 134 // send end message
Kize 0:4289fbe66d45 135 rospc.putc(endmsg);
Kize 0:4289fbe66d45 136 //wait_ms(pub_wait_time);
Kize 0:4289fbe66d45 137 }
Kize 0:4289fbe66d45 138
Kize 0:4289fbe66d45 139 void Mbedserial::char_write(char *array, int arraysize)
Kize 0:4289fbe66d45 140 {
Kize 0:4289fbe66d45 141 // send data type
Kize 0:4289fbe66d45 142 char msg_type = 'c';
Kize 0:4289fbe66d45 143 rospc.putc(msg_type);
Kize 0:4289fbe66d45 144
Kize 0:4289fbe66d45 145 // send array size
Kize 0:4289fbe66d45 146 char arraysize_c[4];
Kize 0:4289fbe66d45 147 *(int *)arraysize_c = arraysize;
Kize 0:4289fbe66d45 148 //memcpy(arraysize_c, &arraysize, 4);
Kize 0:4289fbe66d45 149 for (int i = 0; i < 4; i++)
Kize 0:4289fbe66d45 150 {
Kize 0:4289fbe66d45 151 rospc.putc(arraysize_c[i]);
Kize 0:4289fbe66d45 152 }
Kize 0:4289fbe66d45 153
Kize 0:4289fbe66d45 154 // send char data
Kize 0:4289fbe66d45 155 for (int i = 0; i < arraysize; i++)
Kize 0:4289fbe66d45 156 {
Kize 0:4289fbe66d45 157 rospc.putc(array[i]);
Kize 0:4289fbe66d45 158 }
Kize 0:4289fbe66d45 159
Kize 0:4289fbe66d45 160 // send end message
Kize 0:4289fbe66d45 161 rospc.putc(endmsg);
Kize 0:4289fbe66d45 162 //wait_ms(pub_wait_time);
Kize 0:4289fbe66d45 163 }