22 #include "imu_cruizcore_xg1010.h"
24 #include <core/threading/mutex.h>
25 #include <core/threading/mutex_locker.h>
27 #include <utils/math/angle.h>
28 #ifdef USE_TIMETRACKER
29 # include <utils/time/tracker.h>
31 #include <utils/time/tracker_macros.h>
33 #include <boost/bind/bind.hpp>
41 #define RECONNECT_INTERVAL 2000
50 #if BOOST_VERSION < 104800
53 enum { default_max_transfer_size = 65536 };
55 class transfer_exactly_t
58 typedef std::size_t result_type;
60 explicit transfer_exactly_t(std::size_t size) : size_(size)
64 template <
typename Error>
66 operator()(
const Error &err, std::size_t bytes_transferred)
68 return (!!err || bytes_transferred >= size_)
70 : (size_ - bytes_transferred < default_max_transfer_size
71 ? size_ - bytes_transferred
72 : std::size_t(default_max_transfer_size));
79 inline transfer_exactly_t
80 transfer_exactly(std::size_t size)
82 return transfer_exactly_t(size);
94 std::string &cfg_prefix,
98 io_service_work_(io_service_),
99 deadline_(io_service_)
101 set_name(
"CruizCoreXG1010(%s)", cfg_name.c_str());
107 deadline_.expires_at(boost::posix_time::pos_infin);
114 if (cfg_freq_ != 25 && cfg_freq_ != 50 && cfg_freq_ != 100) {
115 throw Exception(
"Invalid data frequency, must be 25, 50, or 100");
117 if (cfg_baud_rate_ != 115200 && cfg_baud_rate_ != 57600 && cfg_baud_rate_ != 38400
118 && cfg_baud_rate_ != 28800 && cfg_baud_rate_ != 19200 && cfg_baud_rate_ != 9600
119 && cfg_baud_rate_ != 4800) {
123 if ((cfg_freq_ > 25 && cfg_baud_rate_ < 9600) || (cfg_freq_ > 50 && cfg_baud_rate_ < 19200)) {
124 throw Exception(
"Baud rate too low for frequency");
128 receive_timeout_ = (1000 / cfg_freq_) * 2;
142 #ifdef USE_TIMETRACKER
145 ttc_full_loop_ = tt_->add_class(
"Full Loop");
146 ttc_read_ = tt_->add_class(
"Read");
147 ttc_catch_up_ = tt_->add_class(
"Catch up");
148 ttc_parse_ = tt_->add_class(
"Parse");
158 #ifdef USE_TIMETRACKER
166 TIMETRACK_START(ttc_full_loop_);
168 if (serial_.is_open()) {
174 TIMETRACK_START(ttc_read_);
175 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_));
177 ec_ = boost::asio::error::would_block;
180 size_t to_read = CRUIZCORE_XG1010_PACKET_SIZE;
183 if (input_buffer_.size() > 0) {
184 const size_t bsize = input_buffer_.size();
185 size_t full_frames = bsize / CRUIZCORE_XG1010_PACKET_SIZE;
187 CRUIZCORE_XG1010_PACKET_SIZE - (bsize - full_frames * CRUIZCORE_XG1010_PACKET_SIZE);
192 boost::asio::async_read(serial_,
194 #
if BOOST_VERSION >= 104800
195 boost::asio::transfer_exactly(to_read),
196 (boost::lambda::var(ec_) = boost::lambda::_1,
197 boost::lambda::var(bytes_read_) = boost::lambda::_2));
199 transfer_exactly(to_read),
200 boost::bind(&CruizCoreXG1010AcquisitionThread::handle_read,
202 boost::asio::placeholders::error,
203 boost::asio::placeholders::bytes_transferred));
207 io_service_.run_one();
208 while (ec_ == boost::asio::error::would_block);
212 TIMETRACK_END(ttc_read_);
219 if (ec_.value() == boost::system::errc::operation_canceled) {
229 TIMETRACK_START(ttc_catch_up_);
231 bool catch_up =
false;
232 size_t read_size = 0;
234 ec_ = boost::asio::error::would_block;
236 size_t to_read = CRUIZCORE_XG1010_PACKET_SIZE;
238 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_));
239 size_t full_frames = read_size / CRUIZCORE_XG1010_PACKET_SIZE;
240 size_t remaining = CRUIZCORE_XG1010_PACKET_SIZE
241 - (read_size - full_frames * CRUIZCORE_XG1010_PACKET_SIZE);
244 deadline_.expires_from_now(boost::posix_time::microseconds(10));
251 boost::asio::async_read(serial_,
253 #
if BOOST_VERSION >= 104800
254 boost::asio::transfer_exactly(to_read),
255 (boost::lambda::var(ec_) = boost::lambda::_1,
256 boost::lambda::var(bytes_read_) = boost::lambda::_2));
258 transfer_exactly(to_read),
259 boost::bind(&CruizCoreXG1010AcquisitionThread::handle_read,
261 boost::asio::placeholders::error,
262 boost::asio::placeholders::bytes_transferred));
266 io_service_.run_one();
267 while (ec_ == boost::asio::error::would_block);
269 if (bytes_read_ > 0) {
270 read_size += bytes_read_;
271 catch_up = (read_size % CRUIZCORE_XG1010_PACKET_SIZE != 0);
272 ec_ = boost::system::error_code();
274 }
while (bytes_read_ > 0);
277 TIMETRACK_END(ttc_catch_up_);
279 if (ec_ && ec_.value() != boost::system::errc::operation_canceled) {
287 if (input_buffer_.size() >= CRUIZCORE_XG1010_PACKET_SIZE) {
288 TIMETRACK_START(ttc_parse_);
289 if (input_buffer_.size() > CRUIZCORE_XG1010_PACKET_SIZE) {
290 input_buffer_.consume(input_buffer_.size() - CRUIZCORE_XG1010_PACKET_SIZE);
292 std::istream in_stream(&input_buffer_);
293 in_stream.read((
char *)in_packet_, CRUIZCORE_XG1010_PACKET_SIZE);
315 TIMETRACK_END(ttc_parse_);
318 "*** INVALID number of bytes in buffer: %zu\n",
319 input_buffer_.size());
322 }
catch (boost::system::system_error &e) {
323 if (e.code() == boost::asio::error::eof) {
336 usleep(RECONNECT_INTERVAL * 1000);
344 TIMETRACK_END(ttc_full_loop_);
345 #ifdef USE_TIMETRACKER
346 if (++tt_loopcount_ >= 50) {
348 tt_->print_to_stdout();
354 CruizCoreXG1010AcquisitionThread::open_device()
357 input_buffer_.consume(input_buffer_.size());
359 serial_.open(cfg_serial_);
362 boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
363 serial_.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
364 serial_.set_option(boost::asio::serial_port::baud_rate(cfg_baud_rate_));
366 send_init_packet(
true);
369 }
catch (boost::system::system_error &e) {
370 throw Exception(
"CruizCore-XG1010 failed I/O: %s", e.what());
375 CruizCoreXG1010AcquisitionThread::send_init_packet(
bool enable_transfer)
389 if (asprintf(&cmd_packet,
390 "$MIA,I,B,%u,R,%u,D,%s,N* ",
393 enable_transfer ?
"Y" :
"N")
395 throw Exception(
"Failed to create command packet");
398 size_t cmd_packet_len = strlen(cmd_packet);
401 unsigned int checksum = 0;
402 for (
size_t i = 1; i < cmd_packet_len - 3; ++i)
403 checksum += cmd_packet[i];
406 char checksum_str[3];
407 snprintf(checksum_str, 3,
"%X", checksum);
408 cmd_packet[cmd_packet_len - 2] = checksum_str[0];
409 cmd_packet[cmd_packet_len - 1] = checksum_str[1];
411 std::string cmd_packet_s(cmd_packet, cmd_packet_len);
416 boost::asio::write(serial_, boost::asio::buffer(cmd_packet_s.c_str(), cmd_packet_len));
420 CruizCoreXG1010AcquisitionThread::resync()
466 #if BOOST_VERSION >= 104700
467 tcflush(serial_.lowest_layer().native_handle(), TCIOFLUSH);
469 tcflush(serial_.lowest_layer().native(), TCIOFLUSH);
473 const int NUM_TRIES = 10;
474 for (
int t = 1; t <= NUM_TRIES; ++t) {
476 ec_ = boost::asio::error::would_block;
479 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_ * 10));
480 boost::asio::async_read_until(serial_,
482 std::string(
"\xff\xff"),
483 #
if BOOST_VERSION >= 104800
484 (boost::lambda::var(ec_) = boost::lambda::_1,
485 boost::lambda::var(bytes_read_) = boost::lambda::_2));
487 boost::bind(&CruizCoreXG1010AcquisitionThread::handle_read,
489 boost::asio::placeholders::error,
490 boost::asio::placeholders::bytes_transferred));
494 io_service_.run_one();
495 while (ec_ == boost::asio::error::would_block);
498 if (ec_.value() == boost::system::errc::operation_canceled) {
499 throw Exception(
"Timeout (1) on initial synchronization");
501 throw Exception(
"Error (1) on initial synchronization: %s", ec_.message().c_str());
505 input_buffer_.consume(bytes_read_ - 2);
507 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_));
508 ec_ = boost::asio::error::would_block;
510 boost::asio::async_read(serial_,
512 #
if BOOST_VERSION >= 104800
513 boost::asio::transfer_exactly(CRUIZCORE_XG1010_PACKET_SIZE - 2),
514 (boost::lambda::var(ec_) = boost::lambda::_1,
515 boost::lambda::var(bytes_read_) = boost::lambda::_2));
517 transfer_exactly(CRUIZCORE_XG1010_PACKET_SIZE - 2),
518 boost::bind(&CruizCoreXG1010AcquisitionThread::handle_read,
520 boost::asio::placeholders::error,
521 boost::asio::placeholders::bytes_transferred));
525 io_service_.run_one();
526 while (ec_ == boost::asio::error::would_block);
529 if (ec_.value() == boost::system::errc::operation_canceled) {
530 throw Exception(
"Timeout (2) on initial synchronization");
532 throw Exception(
"Error (2) on initial synchronization: %s", ec_.message().c_str());
536 std::istream in_stream(&input_buffer_);
537 in_stream.read((
char *)in_packet_, CRUIZCORE_XG1010_PACKET_SIZE);
541 if (t == NUM_TRIES) {
542 e.
append(
"Resync failed after %d tries", NUM_TRIES);
551 deadline_.expires_at(boost::posix_time::pos_infin);
555 CruizCoreXG1010AcquisitionThread::close_device()
561 CruizCoreXG1010AcquisitionThread::parse_packet()
571 if (in_packet_[0] != 0xFF || in_packet_[1] != 0xFF) {
572 throw Exception(
"Failed to parse packet, invalid header");
575 short int rate = (in_packet_[2] & 0xFF) | ((in_packet_[3] << 8) & 0xFF00);
576 short int angle = (in_packet_[4] & 0xFF) | ((in_packet_[5] << 8) & 0XFF00);
578 int checksum = 0xffff + rate + angle;
579 if (((
unsigned char)(checksum & 0xFF) != in_packet_[6])
580 || ((
unsigned char)((checksum >> 8) & 0xFF) != in_packet_[7])) {
588 throw Exception(
"Failed to parse packet, checksum mismatch");
596 tf::Quaternion q = tf::create_quaternion_from_yaw(-
deg2rad(angle / 100.f));
611 CruizCoreXG1010AcquisitionThread::check_deadline()
613 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
615 deadline_.expires_at(boost::posix_time::pos_infin);
618 #if BOOST_VERSION >= 104800
619 deadline_.async_wait(
620 boost::lambda::bind(&CruizCoreXG1010AcquisitionThread::check_deadline,
this));
622 deadline_.async_wait(boost::bind(&CruizCoreXG1010AcquisitionThread::check_deadline,
this));
virtual void finalize()
Finalize the thread.
CruizCoreXG1010AcquisitionThread(std::string &cfg_name, std::string &cfg_prefix, bool continuous)
Constructor.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
float linear_acceleration_[3]
Pre-allocated linear acceleration as array, 3 entries ordered (x,y,z).
fawkes::Mutex * data_mutex_
Lock while writing to distances or echoes array or marking new data.
double angular_velocity_covariance_[9]
Pre-allocated angular velocity covariance, row major matrix ordered x, y, z.
virtual void loop()
Code to execute in the thread.
float orientation_[4]
Pre-allocated orientation quaternion as array, 4 entries ordered (x,y,z,w).
virtual void finalize()
Finalize the thread.
std::string cfg_prefix_
Configuration path prefix.
bool cfg_continuous_
True if running continuous.
float angular_velocity_[3]
Pre-allocated angular velocities as array, 3 entries ordered (x,y,z).
bool new_data_
Set to true in your loop if new data is available.
virtual void init()
Initialize the thread.
fawkes::Time * timestamp_
Time when the most recent data was received.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
void append(const char *format,...) noexcept
Append messages to the message list.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
const char * name() const
Get name of thread.
void yield()
Yield the processor to another thread or process.
void set_name(const char *format,...)
Set name of thread.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.