Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed the 'cold boot' issue with 14001 razor board #54

Open
wants to merge 23 commits into
base: indigo-devel
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
b7775e9
Update .gitignore
msadowski Oct 17, 2019
be7e2d9
WIP refactoring IMU driver
msadowski Oct 17, 2019
083820b
Write and check the calibration written to the sensor
msadowski Oct 19, 2019
726ee5d
Remove parameters for disabling verification check
msadowski Oct 21, 2019
8e405d9
Fixed the 'cold boot' issue with 14001 razor board
Oct 29, 2019
68332ae
Added an option to ask for the magnetometer information
Oct 29, 2019
33a6940
Publish IMU data, add some settings as parameters
msadowski Oct 30, 2019
c861311
Comment out HW__VERSION_CODE in .ino file
msadowski Oct 30, 2019
2992ee7
Fix whitespace in .ino comment
msadowski Oct 30, 2019
61c30cf
Add empty line at the end of serial_commands.py file
msadowski Oct 30, 2019
5b90142
Merge branch 'master' into feature/publish_magnetometer
joekeo Oct 30, 2019
c23a450
Merge pull request #1 from msadowski/master
joekeo Oct 31, 2019
3bc06fd
Merge branch 'indigo-devel' into feature/publish_magnetometer
joekeo Oct 31, 2019
7daf03b
Added the magnetometer serial command and replaced the line start fro…
Oct 31, 2019
08488e0
added the option to use the magnetometer
Oct 31, 2019
2619156
added magnetometer optional publisher
Oct 31, 2019
4a7c783
added the correct units and axis of the mag according to REP103
Oct 31, 2019
7e7686a
specific changes for the magnetometer yaml and config filein the launch
Oct 31, 2019
3431643
added magnetometer axis explanation
Oct 31, 2019
5893445
Merge remote-tracking branch 'origin/feature/publish_magnetometer' in…
joekeo Oct 31, 2019
dd7ce43
Fix magnetic field orientation
msadowski Nov 6, 2019
412781d
Changed the magnetometer frame according to Mat
Nov 12, 2019
19126a2
Merge branch 'indigo-devel' into fix/magnetometer_orientation
joekeo Nov 12, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1,2 +1,4 @@
*~
config/my_razor.yaml
*.pyc
*.swp
30 changes: 22 additions & 8 deletions config/razor.yaml
Original file line number Diff line number Diff line change
@@ -1,34 +1,48 @@
## USB port
port: /dev/ttyUSB0
port: /dev/ttyACM0
frame_header: "base_imu_link"


##### Calibration ####
### accelerometer
accel_x_min: -250.0
accel_x_max: 250.0
accel_y_min: -250.0
accel_y_max: 250.0
accel_z_min: -250.0
accel_z_max: 250.0

### magnetometer
# standard calibration
magn_x_min: -600.0
magn_x_max: 600.0
magn_y_min: -600.0
magn_y_max: 600.0
magn_z_min: -600.0
magn_z_max: 600.0

# extended calibration
publish_magnetometer: true

# extended mag calibration
calibration_magn_use_extended: false
magn_ellipsoid_center: [0, 0, 0]
magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]

# AHRS to robot calibration
imu_yaw_calibration: 0.0

### gyroscope
gyro_average_offset_x: 0.0
gyro_average_offset_y: 0.0
gyro_average_offset_z: 0.0

# covariance
orientation_covariance: [0.0025, 0, 0,
0, 0.0025, 0,
0, 0, 0.0025]

velocity_covariance: [0.002, 0, 0,
0, 0.002, 0,
0, 0, 0.002]

acceleration_covariance: [0.04, 0, 0,
0, 0.04, 0,
0, 0, 0.04]

magnetic_field_covariance: [0.00, 0, 0,
0, 0.00, 0,
0, 0, 0.00]
2 changes: 1 addition & 1 deletion launch/razor-pub.launch
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<arg name="razor_config_file" default="$(find razor_imu_9dof)/config/my_razor.yaml"/>
<arg name="razor_config_file" default="$(find razor_imu_9dof)/config/razor.yaml"/>
<node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
<rosparam file="$(arg razor_config_file)" command="load"/>
</node>
Expand Down
290 changes: 149 additions & 141 deletions nodes/imu_node.py

Large diffs are not rendered by default.

Empty file added nodes/lib/__init__.py
Empty file.
50 changes: 50 additions & 0 deletions nodes/lib/serial_commands.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
STOP_DATASTREAM = '#o0'
START_DATASTREAM = '#o1'

GET_CALIBRATION_VALUES ='#p'

SET_BINARY_FORMAT = '#ob'
SET_TEXT_FORMAT = '#ot'
SET_TEXT_EXTENDED_FORMAT_NO_MAG = '#ox' # Use this one in the ROS driver WITHOUT magnetometer
SET_TEXT_EXTENDED_FORMAT_WITH_MAG = '#om' # Use this one in the ROS driver WITH magnetometer
SET_CALIBRATION_OUTPUT_MODE = '#oc'
NEXT_SENSOR_CALIBRATION = '#on'

LINE_START_NO_MAG = "#YPRAG="
LINE_START_WITH_MAG = "#YPRAGM="

ENABLE_ERROR_MESSAGE = '#oe1'
DISABLE_ERROR_MESSAGE = '#oe0'
OUTPUT_ERROR_COUNT = '#oec'
OUTPUT_MATH_ERROR_COUNT = '#oem'

SET_CALIB_ACC_X_MIN = '#caxm'
SET_CALIB_ACC_X_MAX = '#caxM'
SET_CALIB_ACC_Y_MIN = '#caym'
SET_CALIB_ACC_Y_MAX = '#cayM'
SET_CALIB_ACC_Z_MIN = '#cazm'
SET_CALIB_ACC_Z_MAX = '#cazM'

SET_MAG_X_MIN = '#cmxm'
SET_MAG_X_MAX = '#cmxM'
SET_MAG_Y_MIN = '#cmym'
SET_MAG_Y_MAX = '#cmyM'
SET_MAG_Z_MIN = '#cmzm'
SET_MAG_Z_MAX = '#cmzM'

SET_MAG_ELLIPSOID_CENTER_0 = '#ccx'
SET_MAG_ELLIPSOID_CENTER_1 = '#ccy'
SET_MAG_ELLIPSOID_CENTER_2 = '#ccz'
SET_MAG_ELLIPSOID_TRANSFORM_0_0 = '#ctxX'
SET_MAG_ELLIPSOID_TRANSFORM_0_1 = '#ctxY'
SET_MAG_ELLIPSOID_TRANSFORM_0_2 = '#ctxZ'
SET_MAG_ELLIPSOID_TRANSFORM_1_0 = '#ctyX'
SET_MAG_ELLIPSOID_TRANSFORM_1_1 = '#ctyY'
SET_MAG_ELLIPSOID_TRANSFORM_1_2 = '#ctyZ'
SET_MAG_ELLIPSOID_TRANSFORM_2_0 = '#ctzX'
SET_MAG_ELLIPSOID_TRANSFORM_2_1 = '#ctzY'
SET_MAG_ELLIPSOID_TRANSFORM_2_2 = '#ctzZ'

SET_GYRO_AVERAGE_OFFSET_X = '#cgx'
SET_GYRO_AVERAGE_OFFSET_Y = '#cgy'
SET_GYRO_AVERAGE_OFFSET_Z = '#cgz'
22 changes: 21 additions & 1 deletion src/Razor_AHRS/Output.ino
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,27 @@ void output_both_angles_and_sensors_text()
LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.println();
}

void output_both_angles_and_all_sensors_text()
{
LOG_PORT.print("#YPRAGM=");
LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(",");
LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(",");
LOG_PORT.print(TO_DEG(roll)); LOG_PORT.print(",");

LOG_PORT.print(Accel_Vector[0]); LOG_PORT.print(",");
LOG_PORT.print(Accel_Vector[1]); LOG_PORT.print(",");
LOG_PORT.print(Accel_Vector[2]); LOG_PORT.print(",");

LOG_PORT.print(Gyro_Vector[0]); LOG_PORT.print(",");
LOG_PORT.print(Gyro_Vector[1]); LOG_PORT.print(",");
LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.print(",");

//TODO:Check units and calibration
LOG_PORT.print(magnetom[0]); LOG_PORT.print(",");
LOG_PORT.print(magnetom[1]); LOG_PORT.print(",");
LOG_PORT.print(magnetom[2]); LOG_PORT.println();
}

void output_sensors_binary()
{
LOG_PORT.write((byte*) accel, 12);
Expand Down Expand Up @@ -145,4 +166,3 @@ void output_sensors()
}
}
}

54 changes: 50 additions & 4 deletions src/Razor_AHRS/Razor_AHRS.ino
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,12 @@
velocity is in rad/s. (Output frames have form like
"#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01",
followed by carriage return and line feed [\r\n]).
"#om" - Output angles and linear acceleration, rotational
velocity and magnetic field. Angles are in degrees, acceleration is
in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational
velocity is in rad/s. (Output frames have form like
"#YPRAGM=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01,1.0,2.0,3.0",
followed by carriage return and line feed [\r\n]).

// Sensor calibration
"#oc" - Go to CALIBRATION output mode.
Expand Down Expand Up @@ -208,7 +214,7 @@
// Select your hardware here by uncommenting one line!
//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
//#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
//#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001"
#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001"
//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
//#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
Expand Down Expand Up @@ -238,6 +244,7 @@
#define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes
#define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes
#define OUTPUT__MODE_ANGLES_AG_SENSORS 5 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel
#define OUTPUT__MODE_ANGLES_AGM_SENSORS 6 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel + magnetometer data
// Output format definitions (do not change)
#define OUTPUT__FORMAT_TEXT 0 // Outputs data as text
#define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float
Expand Down Expand Up @@ -629,12 +636,25 @@ char readChar()

void setup()
{
// Init serial output
LOG_PORT.begin(OUTPUT__BAUD_RATE);

// Init status LED
pinMode (STATUS_LED_PIN, OUTPUT);
digitalWrite(STATUS_LED_PIN, LOW);
double time_old = 0;
// Init serial output
LOG_PORT.end();
delay(50);
LOG_PORT.begin(OUTPUT__BAUD_RATE);
while(!LOG_PORT){
if (millis() - time_old > 10000){
time_old = millis();
digitalWrite(STATUS_LED_PIN, HIGH);
delay(50);
#if HW__VERSION_CODE == 14001
NVIC_SystemReset(); // processor software reset
#endif
}
}


// Init sensors
delay(50); // Give sensors enough time to start
Expand Down Expand Up @@ -672,6 +692,9 @@ void loop()
{
// Read incoming control messages
#if HW__VERSION_CODE == 14001
if (!LOG_PORT)
NVIC_SystemReset(); // processor software reset

// Compatibility fix : if bytes are sent 1 by 1 without being read, available() might never return more than 1...
// Therefore, we need to read bytes 1 by 1 and the command byte needs to be a blocking read...
if (LOG_PORT.available() >= 1)
Expand Down Expand Up @@ -728,6 +751,11 @@ void loop()
output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS;
output_format = OUTPUT__FORMAT_TEXT;
}
else if (output_param == 'm') // Output angles + accel + rot. vel as te_x_t
{
output_mode = OUTPUT__MODE_ANGLES_AGM_SENSORS;
output_format = OUTPUT__FORMAT_TEXT;
}
else if (output_param == 's') // Output _s_ensor values
{
char values_param = readChar();
Expand Down Expand Up @@ -1013,6 +1041,24 @@ void loop()

if (output_stream_on || output_single_on) output_both_angles_and_sensors_text();
}
else if (output_mode == OUTPUT__MODE_ANGLES_AGM_SENSORS) // Output angles + accel + rot. vel
{
// Apply sensor calibration
compensate_sensor_errors();

#if DEBUG__USE_ONLY_DMP_M0 == true
Euler_angles_only_DMP_M0();
#else
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
#endif // DEBUG__USE_ONLY_DMP_M0

if (output_stream_on || output_single_on) output_both_angles_and_all_sensors_text();
}
else // Output sensor values
{
if (output_stream_on || output_single_on) output_sensors();
Expand Down