Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox:[517] gearbox/trunk/src/hokuyo_aist
|
From: <gb...@us...> - 2010-09-14 00:40:00
|
Revision: 517
http://gearbox.svn.sourceforge.net/gearbox/?rev=517&view=rev
Author: gbiggs
Date: 2010-09-14 00:39:54 +0000 (Tue, 14 Sep 2010)
Log Message:
-----------
Fixed a divide-by-zero error and a bug in timestamp calculation
Modified Paths:
--------------
gearbox/trunk/src/hokuyo_aist/scan_data.cpp
gearbox/trunk/src/hokuyo_aist/sensor.cpp
gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
gearbox/trunk/src/hokuyo_aist/sensor_info.h
gearbox/trunk/src/hokuyo_aist/test/example.cpp
Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-09-07 06:39:17 UTC (rev 516)
+++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-09-14 00:39:54 UTC (rev 517)
@@ -312,8 +312,8 @@
}
else
ss << "No data errors.\n";
- ss << "Laser time stamp: " << laser_time_ << '\n';
- ss << "System time stamp: " << system_time_ << '\n';
+ ss << "Laser time stamp: " << laser_time_ << "ms\n";
+ ss << "System time stamp: " << system_time_ << "ns\n";
return ss.str();
}
Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-09-07 06:39:17 UTC (rev 516)
+++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-09-14 00:39:54 UTC (rev 517)
@@ -812,6 +812,7 @@
enable_checksum_workaround_ = false;
info.calculate_values();
+ time_resolution_ = info.time_resolution;
if(verbose_)
{
err_output_ << "Retrieved sensor info:\n";
@@ -858,7 +859,10 @@
unsigned int samples)
{
if(verbose_)
- err_output_ << "Entering timing mode.\n";
+ {
+ err_output_ << "Entering timing mode; start system time is " <<
+ get_computer_time() << "ns.\n";
+ }
enter_timing_mode();
// From A. Carballo, Y. Hara, H. Kawata, T.
@@ -884,6 +888,15 @@
wrap_timestamp(get_timing_mode_time(&end_time)) * 1e6;
offsets.push_back(start_time -
(laser_time - (end_time - start_time) / 2));
+ if(verbose_)
+ {
+ err_output_ << "Offset #" << ii << " start_time = " << start_time;
+ err_output_ << "\tend_time = " << end_time;
+ err_output_ << "\tlaser_time = " << laser_time;
+ err_output_ << "\tCalculated offset = " <<
+ (start_time - (laser_time - (end_time - start_time) / 2)) <<
+ '\n';
+ }
}
// Calculate the median offset
time_offset_ = median(offsets);
@@ -1785,6 +1798,10 @@
/// timestamp must be in milliseconds. The result is in nanoseconds.
unsigned long long Sensor::offset_timestamp(unsigned int timestamp)
{
+ std::cerr << "time_offset_ = " << time_offset_ << '\n';
+ std::cerr << "timestamp = " << timestamp << '\n';
+ std::cerr << "time_drift_rate_ = " << time_drift_rate_ << '\n';
+ std::cerr << "time_skew_alpha_ = " << time_skew_alpha_ << '\n';
return ((1 - time_drift_rate_) * timestamp * 1e6 + time_offset_) /
(1 - time_skew_alpha_);
}
@@ -1792,6 +1809,9 @@
unsigned int Sensor::step_to_time_offset(int start_step)
{
+ std::cerr << "Converting step to time offset for step " << start_step;
+ std::cerr << " and resolution " << time_resolution_ << ": ";
+ std::cerr << first_step_ * time_resolution_ << '\n';
if(start_step < 0)
return first_step_ * time_resolution_;
else
@@ -2050,7 +2070,6 @@
throw ParseError(buffer, "Motor speed");
}
info.speed_level = 0;
- time_resolution_ = info.speed;
}
else
{
@@ -2061,7 +2080,6 @@
{
throw ParseError(buffer, "Motor speed");
}
- time_resolution_ = info.speed;
}
}
else if(strncmp(buffer, "MESM", 4) == 0)
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-09-07 06:39:17 UTC (rev 516)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-09-14 00:39:54 UTC (rev 517)
@@ -113,7 +113,18 @@
max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) *
resolution;
scanable_steps = last_step - first_step + 1;
- time_resolution = 60000 / speed;
+ // Calculate the time taken for a single scan.
+ if(speed == 0)
+ {
+ time_resolution = 60000.0 / 600.0;
+ }
+ else
+ {
+ // 60000 = milliseconds in a minute
+ // speed is in RPM
+ time_resolution = 60000.0 / speed;
+ }
+ time_resolution /= static_cast<double>(steps);
}
@@ -146,6 +157,7 @@
ss << "Motor speed: " << speed << "rpm (level " << speed_level <<
")\tBaud rate: " << baud << "bps\n";
ss << "Time stamp: " << time << "ms\n";
+ ss << "Time between scan points: " << time_resolution << "ms\n";
ss << "Sensor diagnostic: " << sensor_diagnostic << '\n';
return ss.str();
Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-09-07 06:39:17 UTC (rev 516)
+++ gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-09-14 00:39:54 UTC (rev 517)
@@ -197,7 +197,7 @@
/// Angle between two scan points (radians).
double resolution;
/// Time between two scan points (milliseconds).
- unsigned int time_resolution;
+ double time_resolution;
/// Total number of steps in a full scan (lastStep - firstStep).
unsigned int scanable_steps;
/// Absolute maximum commandable step.
Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/test/example.cpp 2010-09-07 06:39:17 UTC (rev 516)
+++ gearbox/trunk/src/hokuyo_aist/test/example.cpp 2010-09-14 00:39:54 UTC (rev 517)
@@ -115,6 +115,14 @@
// Open the laser
laser.open(port_options);
+
+ // Calibrate the laser time stamp
+ std::cout << "Calibrating laser time\n";
+ laser.calibrate_time();
+ std::cout << "Calculated offset: " << laser.time_offset() << "ns\n";
+ std::cout << "Calculated drift rate: " << laser.drift_rate() << '\n';
+ std::cout << "Calculated skew alpha: " << laser.skew_alpha() << '\n';
+
// Turn the laser on
laser.set_power(true);
// Set the baud rate
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|