dbus.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, Qiayuan Liao
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 //
35 // Created by qiayuan on 2019/10/30.
36 //
37 
38 #include <rm_dbus/dbus.h>
39 #include <ros/ros.h>
40 
41 #include <fcntl.h> /* File control definitions */
42 #include <cstring>
43 #include <unistd.h>
44 
45 #define termios asmtermios
46 #include <asm/termios.h>
47 #undef termios
48 
49 #include <termios.h>
50 
51 extern "C" {
52 extern int ioctl(int __fd, unsigned long int __request, ...) throw();
53 }
54 
55 void DBus::init(const char* serial)
56 {
57  int fd = open(serial, O_RDWR | O_NOCTTY | O_SYNC);
58 
59  struct termios2 options
60  {
61  };
62  ioctl(fd, TCGETS2, &options);
63 
64  if (fd == -1)
65  {
66  ROS_ERROR("[rt_dbus] Unable to open dbus\n");
67  }
68 
69  // Even parity(8E1):
70  options.c_cflag &= ~CBAUD;
71  options.c_cflag |= BOTHER;
72 
73  options.c_cflag |= PARENB;
74  options.c_cflag &= ~PARODD;
75  options.c_cflag &= ~CSTOPB;
76  options.c_cflag &= ~CSIZE;
77  options.c_cflag |= CS8;
78 
79  options.c_ispeed = 100000;
80  options.c_ospeed = 100000;
81  options.c_iflag &= ~(IXON | IXOFF | IXANY);
82  options.c_iflag &= ~IGNBRK; // disable break processing
83 
84  /* set input mode (non−canonical, no echo,...) */
85  options.c_lflag = 0;
86  options.c_cc[VTIME] = 0;
87  options.c_cc[VMIN] = 0;
88 
89  options.c_oflag = 0; // no remapping, no delays
90  options.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, enable reading
91  ioctl(fd, TCSETS2, &options);
92 
93  port_ = fd;
94 }
95 
96 void DBus::read()
97 {
98  uint8_t read_byte;
99  int timeout = 0; // time out of one package
100  int count = 0; // count of bit of one package
101  while (timeout < 10)
102  {
103  // Read a byte //
104  size_t n = ::read(port_, &read_byte, sizeof(read_byte));
105  if (n == 0)
106  {
107  timeout++;
108  }
109  else if (n == 1)
110  {
111  // Shift the buffer //
112  for (int i = 0; i < 17; i++)
113  {
114  buff_[i] = buff_[i + 1];
115  }
116  buff_[17] = read_byte;
117  count++;
118  }
119  }
120  unpack();
121  if (count < 17)
122  {
123  memset(&d_bus_data_, 0, sizeof(d_bus_data_));
124  is_update_ = false;
125  }
126  else
127  {
128  is_update_ = true;
129  }
130 }
131 
132 void DBus::unpack()
133 {
134  d_bus_data_.ch0 = (buff_[0] | buff_[1] << 8) & 0x07FF;
135  d_bus_data_.ch0 -= 1024;
136  d_bus_data_.ch1 = (buff_[1] >> 3 | buff_[2] << 5) & 0x07FF;
137  d_bus_data_.ch1 -= 1024;
138  d_bus_data_.ch2 = (buff_[2] >> 6 | buff_[3] << 2 | buff_[4] << 10) & 0x07FF;
139  d_bus_data_.ch2 -= 1024;
140  d_bus_data_.ch3 = (buff_[4] >> 1 | buff_[5] << 7) & 0x07FF;
141  d_bus_data_.ch3 -= 1024;
142  /* prevent remote control zero deviation */
143  if (d_bus_data_.ch0 <= 10 && d_bus_data_.ch0 >= -10)
144  d_bus_data_.ch0 = 0;
145  if (d_bus_data_.ch1 <= 10 && d_bus_data_.ch1 >= -10)
146  d_bus_data_.ch1 = 0;
147  if (d_bus_data_.ch2 <= 10 && d_bus_data_.ch2 >= -10)
148  d_bus_data_.ch2 = 0;
149  if (d_bus_data_.ch3 <= 10 && d_bus_data_.ch3 >= -10)
150  d_bus_data_.ch3 = 0;
151 
152  d_bus_data_.s0 = ((buff_[5] >> 4) & 0x0003);
153  d_bus_data_.s1 = ((buff_[5] >> 4) & 0x000C) >> 2;
154 
155  if ((abs(d_bus_data_.ch0) > 660) || (abs(d_bus_data_.ch1) > 660) || (abs(d_bus_data_.ch2) > 660) ||
156  (abs(d_bus_data_.ch3) > 660))
157  {
158  is_success = false;
159  return;
160  }
161  d_bus_data_.x = buff_[6] | (buff_[7] << 8);
162  d_bus_data_.y = buff_[8] | (buff_[9] << 8);
163  d_bus_data_.z = buff_[10] | (buff_[11] << 8);
164  d_bus_data_.l = buff_[12];
165  d_bus_data_.r = buff_[13];
166  d_bus_data_.key = buff_[14] | buff_[15] << 8; // key board code
167  d_bus_data_.wheel = (buff_[16] | buff_[17] << 8) - 1024;
168  is_success = true;
169 }
170 
171 void DBus::getData(rm_msgs::DbusData& d_bus_data) const
172 {
173  if (is_success)
174  {
175  d_bus_data.ch_r_x = static_cast<double>(d_bus_data_.ch0 / 660.0);
176  d_bus_data.ch_r_y = static_cast<double>(d_bus_data_.ch1 / 660.0);
177  d_bus_data.ch_l_x = static_cast<double>(d_bus_data_.ch2 / 660.0);
178  d_bus_data.ch_l_y = static_cast<double>(d_bus_data_.ch3 / 660.0);
179  d_bus_data.m_x = static_cast<double>(d_bus_data_.x / 1600.0);
180  d_bus_data.m_y = static_cast<double>(d_bus_data_.y / 1600.0);
181  d_bus_data.m_z = static_cast<double>(d_bus_data_.z / 1600.0);
182  d_bus_data.wheel = static_cast<double>(d_bus_data_.wheel / 660.0);
183 
184  if (d_bus_data_.s1 != 0)
185  d_bus_data.s_l = d_bus_data_.s1;
186  if (d_bus_data_.s0 != 0)
187  d_bus_data.s_r = d_bus_data_.s0;
188  d_bus_data.p_l = d_bus_data_.l;
189  d_bus_data.p_r = d_bus_data_.r;
190 
191  d_bus_data.key_w = d_bus_data_.key & 0x01 ? true : false;
192  d_bus_data.key_s = d_bus_data_.key & 0x02 ? true : false;
193  d_bus_data.key_a = d_bus_data_.key & 0x04 ? true : false;
194  d_bus_data.key_d = d_bus_data_.key & 0x08 ? true : false;
195  d_bus_data.key_shift = d_bus_data_.key & 0x10 ? true : false;
196  d_bus_data.key_ctrl = d_bus_data_.key & 0x20 ? true : false;
197  d_bus_data.key_q = d_bus_data_.key & 0x40 ? true : false;
198  d_bus_data.key_e = d_bus_data_.key & 0x80 ? true : false;
199  d_bus_data.key_r = (d_bus_data_.key >> 8) & 0x01 ? true : false;
200  d_bus_data.key_f = (d_bus_data_.key >> 8) & 0x02 ? true : false;
201  d_bus_data.key_g = (d_bus_data_.key >> 8) & 0x04 ? true : false;
202  d_bus_data.key_z = (d_bus_data_.key >> 8) & 0x08 ? true : false;
203  d_bus_data.key_x = (d_bus_data_.key >> 8) & 0x10 ? true : false;
204  d_bus_data.key_c = (d_bus_data_.key >> 8) & 0x20 ? true : false;
205  d_bus_data.key_v = (d_bus_data_.key >> 8) & 0x40 ? true : false;
206  d_bus_data.key_b = (d_bus_data_.key >> 8) & 0x80 ? true : false;
207  if (is_update_)
208  d_bus_data.stamp = ros::Time::now();
209  }
210 }
DBusData_t::key
uint16_t key
Definition: dbus.h:91
DBusData_t::s0
uint8_t s0
Definition: dbus.h:81
DBusData_t::z
int16_t z
Definition: dbus.h:87
DBusData_t::r
uint8_t r
Definition: dbus.h:90
DBusData_t::s1
uint8_t s1
Definition: dbus.h:82
ioctl
int ioctl(int __fd, unsigned long int __request,...)
dbus.h
ros.h
DBusData_t::x
int16_t x
Definition: dbus.h:85
DBusData_t::ch2
int16_t ch2
Definition: dbus.h:79
DBusData_t::y
int16_t y
Definition: dbus.h:86
DBus::init
void init(const char *serial)
Definition: dbus.cpp:55
DBusData_t::ch0
int16_t ch0
Definition: dbus.h:77
DBus::d_bus_data_
DBusData_t d_bus_data_
Definition: dbus.h:74
DBus::getData
void getData(rm_msgs::DbusData &d_bus_data) const
Definition: dbus.cpp:171
DBus::port_
int port_
Definition: dbus.h:75
DBusData_t::ch3
int16_t ch3
Definition: dbus.h:80
DBusData_t::l
uint8_t l
Definition: dbus.h:89
DBus::unpack
void unpack()
Definition: dbus.cpp:132
DBusData_t::wheel
int16_t wheel
Definition: dbus.h:83
DBusData_t::ch1
int16_t ch1
Definition: dbus.h:78
ROS_ERROR
#define ROS_ERROR(...)
DBus::is_success
bool is_success
Definition: dbus.h:77
DBus::read
void read()
Definition: dbus.cpp:96
DBus::buff_
int16_t buff_[18]
Definition: dbus.h:76
DBus::is_update_
bool is_update_
Definition: dbus.h:78
ros::Time::now
static Time now()

rm_dbus
Author(s): Qiayuan Liao
autogenerated on Thu Apr 24 2025 02:21:21