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.

Sign up for GitHub

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

Jump to bottom

Added support of SEN-14001 (9DoF Razor IMU M0) as well as ROS-specific output, also added in 2020 support for "OpenLog Artemis": SPX-15846 and DEV-16832 #64

Open
wants to merge 26 commits into
base: master
Choose a base branch
from
Open

Added support of SEN-14001 (9DoF Razor IMU M0) as well as ROS-specific output, also added in 2020 support for "OpenLog Artemis": SPX-15846 and DEV-16832 #64

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
241e56e
Added support of SEN-14001 (*9DoF Razor IMU M0*)
Nov 19, 2017
7a9bcb2
Updated README
Nov 19, 2017
762a47f
Updated README
Nov 19, 2017
dcf01e2
Added comments about the conversions in Sensors.ino
Nov 20, 2017
29d7718
Added error count in Sensors.ino
Nov 20, 2017
cc615fd
Minor correction in Sensors.ino
Nov 24, 2017
9c743d9
Added ROS-compatible output mode
Nov 24, 2017
b943157
Updated README
Nov 24, 2017
249b2b6
Added ROS-compatible input mode to set calibration parameters
Nov 24, 2017
55b0efc
Updated README
Nov 24, 2017
16e1e0f
Updated README
Nov 24, 2017
54e85af
Updated README
Nov 24, 2017
63ff26f
Fixed the problem where commands were ignored by the M0 depending on …
Nov 27, 2017
3cf447c
Initialized some unitialized global variables
Nov 27, 2017
af35512
Added an option to get yaw/pitch/roll from the M0 DMP and a command t…
Dec 5, 2017
af77487
Added various options to determine the most stable configuration for …
Dec 6, 2017
fcb68f5
Removed some unnecessary math error checking, set back gyro full-scal…
Dec 7, 2017
a115744
Calibration data are now also used to compute the initial orientation
Dec 7, 2017
2520ab9
Updated README and comments
Dec 20, 2017
8020c61
Added Windows compatibility to C++ sample
Dec 26, 2017
8668656
Added Visual Studio 64 bit compatibility to C++ sample
Dec 27, 2017
9850562
Updated Windows compatibility of C++ sample
Dec 27, 2017
5c179de
Updated README
Dec 27, 2017
71f2593
Added comment about LOG_PORT SERIAL_PORT_HARDWARE
lebarsfa Oct 10, 2020
6f20e11
Added support for "OpenLog Artemis": DEV-16832
lebarsfa Dec 20, 2020
49e9e3d
Support for "OpenLog Artemis": SPX-15846 (retired version, not tested)
lebarsfa Dec 28, 2020
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
12 changes: 6 additions & 6 deletions Arduino/Razor_AHRS/Compass.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,12 @@

void Compass_Heading()
{
float mag_x;
float mag_y;
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
float mag_x = 0;
float mag_y = 0;
float cos_roll = 0;
float sin_roll = 0;
float cos_pitch = 0;
float sin_pitch = 0;

cos_roll = cos(roll);
sin_roll = sin(roll);
Expand Down
58 changes: 30 additions & 28 deletions Arduino/Razor_AHRS/DCM.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,14 @@ void Normalize(void)
/**************************************************/
void Drift_correction(void)
{
float mag_heading_x;
float mag_heading_y;
float errorCourse;
float mag_heading_x = 0;
float mag_heading_y = 0;
float errorCourse = 0;
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Accel_magnitude = 0;
float Accel_weight = 0;


//*****Roll and Pitch***************
Expand Down Expand Up @@ -85,27 +85,30 @@ void Matrix_update(void)
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#else // Use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#endif
if (DEBUG__NO_DRIFT_CORRECTION) // Do not use drift correction
{
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2] = G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0] = G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt*Gyro_Vector[0];
Update_Matrix[2][0] = -G_Dt*Gyro_Vector[1];
Update_Matrix[2][1] = G_Dt*Gyro_Vector[0];
Update_Matrix[2][2] = 0;
}
else // Use drift correction
{
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2] = G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0] = G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0] = -G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1] = G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2] = 0;
}

Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c

Expand All @@ -120,8 +123,7 @@ void Matrix_update(void)

void Euler_angles(void)
{
pitch = -asin(DCM_Matrix[2][0]);
if ((DCM_Matrix[2][0] < -1)||(DCM_Matrix[2][0] > 1)) num_math_errors++; else pitch = -asin(DCM_Matrix[2][0]); // Attempt to prevent nan problems...
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}

92 changes: 54 additions & 38 deletions Arduino/Razor_AHRS/Output.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,14 @@ void output_angles()
ypr[0] = TO_DEG(yaw);
ypr[1] = TO_DEG(pitch);
ypr[2] = TO_DEG(roll);
Serial.write((byte*) ypr, 12); // No new-line
LOG_PORT.write((byte*) ypr, 12); // No new-line
}
else if (output_format == OUTPUT__FORMAT_TEXT)
{
Serial.print("#YPR=");
Serial.print(TO_DEG(yaw)); Serial.print(",");
Serial.print(TO_DEG(pitch)); Serial.print(",");
Serial.print(TO_DEG(roll)); Serial.println();
LOG_PORT.print("#YPR=");
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.println();
}
}

Expand All @@ -25,29 +25,29 @@ void output_calibration(int calibration_sensor)
if (calibration_sensor == 0) // Accelerometer
{
// Output MIN/MAX values
Serial.print("accel x,y,z (min/max) = ");
LOG_PORT.print("accel x,y,z (min/max) = ");
for (int i = 0; i < 3; i++) {
if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
Serial.print(accel_min[i]);
Serial.print("/");
Serial.print(accel_max[i]);
if (i < 2) Serial.print(" ");
else Serial.println();
LOG_PORT.print(accel_min[i]);
LOG_PORT.print("/");
LOG_PORT.print(accel_max[i]);
if (i < 2) LOG_PORT.print(" ");
else LOG_PORT.println();
}
}
else if (calibration_sensor == 1) // Magnetometer
{
// Output MIN/MAX values
Serial.print("magn x,y,z (min/max) = ");
LOG_PORT.print("magn x,y,z (min/max) = ");
for (int i = 0; i < 3; i++) {
if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
Serial.print(magnetom_min[i]);
Serial.print("/");
Serial.print(magnetom_max[i]);
if (i < 2) Serial.print(" ");
else Serial.println();
LOG_PORT.print(magnetom_min[i]);
LOG_PORT.print("/");
LOG_PORT.print(magnetom_max[i]);
if (i < 2) LOG_PORT.print(" ");
else LOG_PORT.println();
}
}
else if (calibration_sensor == 2) // Gyroscope
Expand All @@ -58,40 +58,56 @@ void output_calibration(int calibration_sensor)
gyro_num_samples++;

// Output current and averaged gyroscope values
Serial.print("gyro x,y,z (current/average) = ");
LOG_PORT.print("gyro x,y,z (current/average) = ");
for (int i = 0; i < 3; i++) {
Serial.print(gyro[i]);
Serial.print("/");
Serial.print(gyro_average[i] / (float) gyro_num_samples);
if (i < 2) Serial.print(" ");
else Serial.println();
LOG_PORT.print(gyro[i]);
LOG_PORT.print("/");
LOG_PORT.print(gyro_average[i] / (float) gyro_num_samples);
if (i < 2) LOG_PORT.print(" ");
else LOG_PORT.println();
}
}
}

void output_sensors_text(char raw_or_calibrated)
{
Serial.print("#A-"); Serial.print(raw_or_calibrated); Serial.print('=');
Serial.print(accel[0]); Serial.print(",");
Serial.print(accel[1]); Serial.print(",");
Serial.print(accel[2]); Serial.println();
LOG_PORT.print("#A-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
LOG_PORT.print(accel[0]); LOG_PORT.print(",");
LOG_PORT.print(accel[1]); LOG_PORT.print(",");
LOG_PORT.print(accel[2]); LOG_PORT.println();

Serial.print("#M-"); Serial.print(raw_or_calibrated); Serial.print('=');
Serial.print(magnetom[0]); Serial.print(",");
Serial.print(magnetom[1]); Serial.print(",");
Serial.print(magnetom[2]); Serial.println();
LOG_PORT.print("#M-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
LOG_PORT.print(magnetom[0]); LOG_PORT.print(",");
LOG_PORT.print(magnetom[1]); LOG_PORT.print(",");
LOG_PORT.print(magnetom[2]); LOG_PORT.println();

Serial.print("#G-"); Serial.print(raw_or_calibrated); Serial.print('=');
Serial.print(gyro[0]); Serial.print(",");
Serial.print(gyro[1]); Serial.print(",");
Serial.print(gyro[2]); Serial.println();
LOG_PORT.print("#G-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
LOG_PORT.print(gyro[0]); LOG_PORT.print(",");
LOG_PORT.print(gyro[1]); LOG_PORT.print(",");
LOG_PORT.print(gyro[2]); LOG_PORT.println();
}

void output_both_angles_and_sensors_text()
{
LOG_PORT.print("#YPRAG=");
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.println();
}

void output_sensors_binary()
{
Serial.write((byte*) accel, 12);
Serial.write((byte*) magnetom, 12);
Serial.write((byte*) gyro, 12);
LOG_PORT.write((byte*) accel, 12);
LOG_PORT.write((byte*) magnetom, 12);
LOG_PORT.write((byte*) gyro, 12);
}

void output_sensors()
Expand Down
Loading