22 #include "imu_cruizcore_xg1010.h" 24 #include <core/threading/mutex.h> 25 #include <core/threading/mutex_locker.h> 28 #include <utils/math/angle.h> 29 #ifdef USE_TIMETRACKER 30 # include <utils/time/tracker.h> 32 #include <utils/time/tracker_macros.h> 34 #include <boost/bind.hpp> 43 #define RECONNECT_INTERVAL 2000 53 #if BOOST_VERSION < 104800 56 enum { default_max_transfer_size = 65536 };
58 class transfer_exactly_t
61 typedef std::size_t result_type;
63 explicit transfer_exactly_t(std::size_t size)
66 template <
typename Error>
67 std::size_t operator()(
const Error& err, std::size_t bytes_transferred)
69 return (!!err || bytes_transferred >= size_) ? 0 :
70 (size_ - bytes_transferred < default_max_transfer_size
71 ? size_ - bytes_transferred : std::size_t(default_max_transfer_size));
78 inline transfer_exactly_t transfer_exactly(std::size_t size)
80 return transfer_exactly_t(size);
93 std::string &cfg_prefix,
96 serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_)
98 set_name(
"CruizCoreXG1010(%s)", cfg_name.c_str());
104 deadline_.expires_at(boost::posix_time::pos_infin);
111 if (cfg_freq_ != 25 && cfg_freq_ != 50 && cfg_freq_ != 100) {
112 throw Exception(
"Invalid data frequency, must be 25, 50, or 100");
114 if (cfg_baud_rate_ != 115200 && cfg_baud_rate_ != 57600 &&
115 cfg_baud_rate_ != 38400 && cfg_baud_rate_ != 28800 &&
116 cfg_baud_rate_ != 19200 && cfg_baud_rate_ != 9600 && cfg_baud_rate_ != 4800)
121 if ( (cfg_freq_ > 25 && cfg_baud_rate_ < 9600) ||
122 (cfg_freq_ > 50 && cfg_baud_rate_ < 19200) )
124 throw Exception(
"Baud rate too low for frequency");
128 receive_timeout_ = (1000 / cfg_freq_) * 2;
141 #ifdef USE_TIMETRACKER 144 ttc_full_loop_ = tt_->add_class(
"Full Loop");
145 ttc_read_ = tt_->add_class(
"Read");
146 ttc_catch_up_ = tt_->add_class(
"Catch up");
147 ttc_parse_ = tt_->add_class(
"Parse");
157 #ifdef USE_TIMETRACKER 166 TIMETRACK_START(ttc_full_loop_);
168 if (serial_.is_open()) {
175 TIMETRACK_START(ttc_read_);
176 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_));
178 ec_ = boost::asio::error::would_block;
181 size_t to_read = CRUIZCORE_XG1010_PACKET_SIZE;
184 if (input_buffer_.size() > 0) {
185 const size_t bsize = input_buffer_.size();
186 size_t full_frames = bsize / CRUIZCORE_XG1010_PACKET_SIZE;
188 CRUIZCORE_XG1010_PACKET_SIZE - (bsize - full_frames * CRUIZCORE_XG1010_PACKET_SIZE);
193 boost::asio::async_read(serial_, input_buffer_,
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),
201 &CruizCoreXG1010AcquisitionThread::handle_read,
203 boost::asio::placeholders::error,
204 boost::asio::placeholders::bytes_transferred
208 do io_service_.run_one();
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 - (read_size - full_frames * CRUIZCORE_XG1010_PACKET_SIZE);
243 deadline_.expires_from_now(boost::posix_time::microseconds(10));
250 boost::asio::async_read(serial_, input_buffer_,
251 #
if BOOST_VERSION >= 104800
252 boost::asio::transfer_exactly(to_read),
253 (boost::lambda::var(ec_) = boost::lambda::_1,
254 boost::lambda::var(bytes_read_) = boost::lambda::_2));
256 transfer_exactly(to_read),
258 &CruizCoreXG1010AcquisitionThread::handle_read,
260 boost::asio::placeholders::error,
261 boost::asio::placeholders::bytes_transferred
265 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
267 if (bytes_read_ > 0) {
268 read_size += bytes_read_;
269 catch_up = (read_size % CRUIZCORE_XG1010_PACKET_SIZE != 0);
270 ec_ = boost::system::error_code();
272 }
while (bytes_read_ > 0);
275 TIMETRACK_END(ttc_catch_up_);
277 if (ec_ && ec_.value() != boost::system::errc::operation_canceled) {
285 if (input_buffer_.size() >= CRUIZCORE_XG1010_PACKET_SIZE) {
286 TIMETRACK_START(ttc_parse_);
287 if (input_buffer_.size() > CRUIZCORE_XG1010_PACKET_SIZE) {
288 input_buffer_.consume(input_buffer_.size() - CRUIZCORE_XG1010_PACKET_SIZE);
290 std::istream in_stream(&input_buffer_);
291 in_stream.read((
char *)in_packet_, CRUIZCORE_XG1010_PACKET_SIZE);
313 TIMETRACK_END(ttc_parse_);
315 logger->
log_warn(
name(),
"*** INVALID number of bytes in buffer: %zu\n", input_buffer_.size());
318 }
catch (boost::system::system_error &e) {
319 if (e.code() == boost::asio::error::eof) {
322 "CruizCoreXG1010 connection lost, trying to re-open");
333 usleep(RECONNECT_INTERVAL * 1000);
340 TIMETRACK_END(ttc_full_loop_);
341 #ifdef USE_TIMETRACKER 342 if (++tt_loopcount_ >= 50) {
344 tt_->print_to_stdout();
351 CruizCoreXG1010AcquisitionThread::open_device()
354 input_buffer_.consume(input_buffer_.size());
356 serial_.open(cfg_serial_);
358 serial_.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
359 serial_.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
360 serial_.set_option(boost::asio::serial_port::baud_rate(cfg_baud_rate_));
362 send_init_packet(
true);
365 }
catch (boost::system::system_error &e) {
366 throw Exception(
"CruizCore-XG1010 failed I/O: %s", e.what());
372 CruizCoreXG1010AcquisitionThread::send_init_packet(
bool enable_transfer)
386 if (asprintf(&cmd_packet,
"$MIA,I,B,%u,R,%u,D,%s,N* ", cfg_baud_rate_, cfg_freq_,
387 enable_transfer ?
"Y" :
"N") == -1)
389 throw Exception(
"Failed to create command packet");
392 size_t cmd_packet_len = strlen(cmd_packet);
395 unsigned int checksum = 0;
396 for(
size_t i = 1; i < cmd_packet_len - 3; ++i ) checksum += cmd_packet[i];
399 char checksum_str[3];
400 snprintf(checksum_str, 3,
"%X", checksum);
401 cmd_packet[cmd_packet_len - 2] = checksum_str[0];
402 cmd_packet[cmd_packet_len - 1] = checksum_str[1];
404 std::string cmd_packet_s(cmd_packet, cmd_packet_len);
409 boost::asio::write(serial_, boost::asio::buffer(cmd_packet_s.c_str(), cmd_packet_len));
414 CruizCoreXG1010AcquisitionThread::resync()
460 #if BOOST_VERSION >= 104700 461 tcflush(serial_.lowest_layer().native_handle(), TCIOFLUSH);
463 tcflush(serial_.lowest_layer().native(), TCIOFLUSH);
467 const int NUM_TRIES = 10;
468 for (
int t = 1; t <= NUM_TRIES; ++t) {
470 ec_ = boost::asio::error::would_block;
473 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_ * 10));
474 boost::asio::async_read_until(serial_, input_buffer_, std::string(
"\xff\xff"),
475 #
if BOOST_VERSION >= 104800
476 (boost::lambda::var(ec_) = boost::lambda::_1,
477 boost::lambda::var(bytes_read_) = boost::lambda::_2));
480 &CruizCoreXG1010AcquisitionThread::handle_read,
482 boost::asio::placeholders::error,
483 boost::asio::placeholders::bytes_transferred
487 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
490 if (ec_.value() == boost::system::errc::operation_canceled) {
491 throw Exception(
"Timeout (1) on initial synchronization");
493 throw Exception(
"Error (1) on initial synchronization: %s", ec_.message().c_str());
497 input_buffer_.consume(bytes_read_ - 2);
499 deadline_.expires_from_now(boost::posix_time::milliseconds(receive_timeout_));
500 ec_ = boost::asio::error::would_block;
502 boost::asio::async_read(serial_, input_buffer_,
503 #
if BOOST_VERSION >= 104800
504 boost::asio::transfer_exactly(CRUIZCORE_XG1010_PACKET_SIZE - 2),
505 (boost::lambda::var(ec_) = boost::lambda::_1,
506 boost::lambda::var(bytes_read_) = boost::lambda::_2));
508 transfer_exactly(CRUIZCORE_XG1010_PACKET_SIZE - 2),
510 &CruizCoreXG1010AcquisitionThread::handle_read,
512 boost::asio::placeholders::error,
513 boost::asio::placeholders::bytes_transferred
517 do io_service_.run_one();
while (ec_ == boost::asio::error::would_block);
520 if (ec_.value() == boost::system::errc::operation_canceled) {
521 throw Exception(
"Timeout (2) on initial synchronization");
523 throw Exception(
"Error (2) on initial synchronization: %s", ec_.message().c_str());
527 std::istream in_stream(&input_buffer_);
528 in_stream.read((
char *)in_packet_, CRUIZCORE_XG1010_PACKET_SIZE);
532 if (t == NUM_TRIES) {
533 e.
append(
"Resync failed after %d tries", NUM_TRIES);
542 deadline_.expires_at(boost::posix_time::pos_infin);
547 CruizCoreXG1010AcquisitionThread::close_device()
553 CruizCoreXG1010AcquisitionThread::parse_packet()
563 if( in_packet_[0] != 0xFF || in_packet_[1] != 0xFF ) {
564 throw Exception(
"Failed to parse packet, invalid header");
567 short int rate = ( in_packet_[2] & 0xFF) | ((in_packet_[3] << 8) & 0xFF00);
568 short int angle = ( in_packet_[4] & 0xFF) | ((in_packet_[5] << 8) & 0XFF00);
570 int checksum = 0xffff + rate + angle;
571 if(((
unsigned char)(checksum & 0xFF) != in_packet_[6]) ||
572 ((
unsigned char)((checksum>>8) & 0xFF) != in_packet_[7]))
581 throw Exception(
"Failed to parse packet, checksum mismatch");
589 tf::Quaternion q = tf::create_quaternion_from_yaw(-
deg2rad(angle / 100.f));
605 CruizCoreXG1010AcquisitionThread::check_deadline()
607 if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now()) {
609 deadline_.expires_at(boost::posix_time::pos_infin);
612 #if BOOST_VERSION >= 104800 613 deadline_.async_wait(boost::lambda::bind(&CruizCoreXG1010AcquisitionThread::check_deadline,
this));
615 deadline_.async_wait(boost::bind(&CruizCoreXG1010AcquisitionThread::check_deadline,
this));
float angular_velocity_[3]
Pre-allocated angular velocities as array, 3 entries ordered (x,y,z).
CruizCoreXG1010AcquisitionThread(std::string &cfg_name, std::string &cfg_prefix, bool continuous)
Constructor.
float orientation_[4]
Pre-allocated orientation quaternion as array, 4 entries ordered (x,y,z,w).
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
Fawkes library namespace.
void unlock()
Unlock the mutex.
fawkes::Time * timestamp_
Time when the most recent data was received.
virtual void loop()
Code to execute in the thread.
Logger * logger
This is the Logger member used to access the logger.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
bool new_data_
Set to true in your loop if new data is available.
virtual void finalize()
Finalize the thread.
std::string cfg_prefix_
Configuration path prefix.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
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.
double angular_velocity_covariance_[9]
Pre-allocated angular velocity covariance, row major matrix ordered x, y, z.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void yield()
Yield the processor to another thread or process.
fawkes::Mutex * data_mutex_
Lock while writing to distances or echoes array or marking new data.
virtual void finalize()
Finalize the thread.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
bool cfg_continuous_
True if running continuous.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float linear_acceleration_[3]
Pre-allocated linear acceleration as array, 3 entries ordered (x,y,z).
Configuration * config
This is the Configuration member used to access the configuration.
virtual void init()
Initialize the thread.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.