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

Use gyro/accel/mag raw values for SCALED_IMU mavlink msg #8517

Closed
wants to merge 9 commits into from
Closed

Use gyro/accel/mag raw values for SCALED_IMU mavlink msg #8517

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
760945d
add unit for raw linear velocities
Dec 22, 2017
4a58563
add unit for raw angular velocities
Dec 22, 2017
3279bd1
mavlink messages: add stream for SCALED_IMU
Dec 22, 2017
0bde4e6
sensor_mag: add units to msg
Dec 22, 2017
bf12eaa
accel/gyro/mag drivers: adapt to new raw msg (unfiltered)
Dec 22, 2017
9258406
posix accel/gyro/mag drivers: adapt to new raw msg (unfiltered)
Dec 22, 2017
c0e2683
qurt accel/gyro/mag drivers: adapt to new raw msg (unfiltered)
Dec 22, 2017
8fba8a2
mavlink receiver: adapt to new raw accel/gyro msg (unfiltered)
Dec 22, 2017
4255442
add missing gyro/mag drivers: adapt to new raw msg (unfiltered)
Dec 27, 2017
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
6 changes: 3 additions & 3 deletions msg/sensor_accel.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@ float32 temperature # temperature in degrees celsius
float32 range_m_s2 # range in m/s^2 (+- this value)
float32 scaling

int16 x_raw
int16 y_raw
int16 z_raw
int16 x_raw # acceleration in the NED X board axis in m/s^2 * 1000
int16 y_raw # acceleration in the NED Y board axis in m/s^2 * 1000
int16 z_raw # acceleration in the NED Z board axis in m/s^2 * 1000
int16 temperature_raw

uint32 device_id # unique device ID for the sensor that does not change between power cycles
6 changes: 3 additions & 3 deletions msg/sensor_gyro.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@ float32 temperature # temperature in degrees celcius
float32 range_rad_s
float32 scaling

int16 x_raw
int16 y_raw
int16 z_raw
int16 x_raw # angular velocity in the NED X board axis in rad/s * 1000
int16 y_raw # angular velocity in the NED Y board axis in rad/s * 1000
int16 z_raw # angular velocity in the NED Z board axis in rad/s * 1000
int16 temperature_raw

uint32 device_id # unique device ID for the sensor that does not change between power cycles
13 changes: 6 additions & 7 deletions msg/sensor_mag.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,14 @@
uint64 error_count
float32 x
float32 y
float32 z
float32 x # [Gauss]
float32 y # [Gauss]
float32 z # [Gauss]
float32 range_ga
float32 scaling
float32 temperature

int16 x_raw
int16 y_raw
int16 z_raw
int16 x_raw # [milli Gauss]
int16 y_raw # [milli Gauss]
int16 z_raw # [milli Gauss]

uint32 device_id # unique device ID for the sensor that does not change between power cycles
bool is_external # if true the mag is external (i.e. not built into the board)

24 changes: 12 additions & 12 deletions src/drivers/adis16448/adis16448.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1437,17 +1437,17 @@ ADIS16448::measure()
grb.error_count = arb.error_count = mrb.error_count = perf_event_count(_bad_transfers);

/* Gyro report: */
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;

float xraw_f = report.gyro_x;
float yraw_f = report.gyro_y;
float zraw_f = report.gyro_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

grb.x_raw = (int16_t)(xraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.y_raw = (int16_t)(yraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.z_raw = (int16_t)(zraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]

float x_gyro_in_new = ((xraw_f * _gyro_range_scale) * M_PI_F / 180.0f - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) * M_PI_F / 180.0f - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) * M_PI_F / 180.0f - _gyro_scale.z_offset) * _gyro_scale.z_scale;
Expand All @@ -1467,17 +1467,17 @@ ADIS16448::measure()
grb.range_rad_s = _gyro_range_rad_s;

/* Accel report: */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;

xraw_f = report.accel_x;
yraw_f = report.accel_y;
zraw_f = report.accel_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

arb.x_raw = (int16_t)(xraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.y_raw = (int16_t)(yraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.z_raw = (int16_t)(zraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]

float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
Expand All @@ -1497,17 +1497,17 @@ ADIS16448::measure()
arb.range_m_s2 = _accel_range_m_s2;

/* Mag report: */
mrb.x_raw = report.mag_x;
mrb.y_raw = report.mag_y;
mrb.z_raw = report.mag_z;

xraw_f = report.mag_x;
yraw_f = report.mag_y;
zraw_f = report.mag_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

mrb.x_raw = (int16_t)(xraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]
mrb.y_raw = (int16_t)(yraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]
mrb.z_raw = (int16_t)(zraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]

float x_mag_new = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
float y_mag_new = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
float z_mag_new = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
Expand Down
5 changes: 5 additions & 0 deletions src/drivers/bma180/bma180.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -725,6 +725,11 @@ BMA180::measure()
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

report.x_raw = (int16_t)(report.x_raw * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
report.y_raw = (int16_t)(report.y_raw * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
report.z_raw = (int16_t)(report.z_raw * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]

report.scaling = _accel_range_scale;
report.range_m_s2 = _accel_range_m_s2;

Expand Down
10 changes: 4 additions & 6 deletions src/drivers/bmi055/bmi055_accel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -739,17 +739,17 @@ BMI055_accel::measure()
*
*/

arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;

float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

arb.x_raw = (int16_t)(xraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.y_raw = (int16_t)(yraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.z_raw = (int16_t)(zraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]

float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
Expand Down Expand Up @@ -863,5 +863,3 @@ BMI055_accel::print_registers()

printf("\n");
}


12 changes: 4 additions & 8 deletions src/drivers/bmi055/bmi055_gyro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -711,17 +711,17 @@ BMI055_gyro::measure()
* 74 from all measurements centers them around zero.
*/

grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;

float xraw_f = report.gyro_x;
float yraw_f = report.gyro_y;
float zraw_f = report.gyro_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

grb.x_raw = (int16_t)(xraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.y_raw = (int16_t)(yraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.z_raw = (int16_t)(zraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]

float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
Expand Down Expand Up @@ -842,7 +842,3 @@ BMI055_gyro::print_registers()

printf("\n");
}




16 changes: 8 additions & 8 deletions src/drivers/bmi160/bmi160.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1161,17 +1161,17 @@ BMI160::measure()

/* NOTE: Axes have been swapped to match the board a few lines above. */

arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;

float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

arb.x_raw = (int16_t)(xraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.y_raw = (int16_t)(yraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
arb.z_raw = (int16_t)(zraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]

float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
Expand Down Expand Up @@ -1199,17 +1199,17 @@ BMI160::measure()
/* return device ID */
arb.device_id = _device_id.devid;

grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;

xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

grb.x_raw = (int16_t)(xraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.y_raw = (int16_t)(yraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
grb.z_raw = (int16_t)(zraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]

float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
Expand Down
4 changes: 4 additions & 0 deletions src/drivers/bmm150/bmm150.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -687,6 +687,10 @@ BMM150::collect()
// apply user specified rotation
rotate_3f(_rotation, mrb.x, mrb.y, mrb.z);

mrb.x_raw = (int16_t)(mrb.x * _range_scale * 1000); // (int16) [Gs * 1000]
mrb.y_raw = (int16_t)(mrb.y * _range_scale * 1000); // (int16) [Gs * 1000]
mrb.z_raw = (int16_t)(mrb.z * _range_scale * 1000); // (int16) [Gs * 1000]


/* Scaling the data */
mrb.x = ((mrb.x * _range_scale) - _scale.x_offset) * _scale.x_scale;
Expand Down
14 changes: 7 additions & 7 deletions src/drivers/fxas21002c/fxas21002c.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1045,17 +1045,17 @@ FXAS21002C::measure()
// whether it has had failures
gyro_report.error_count = perf_event_count(_bad_registers);

gyro_report.x_raw = swap16(raw_gyro_report.x);
gyro_report.y_raw = swap16(raw_gyro_report.y);
gyro_report.z_raw = swap16(raw_gyro_report.z);

float xraw_f = gyro_report.x_raw;
float yraw_f = gyro_report.y_raw;
float zraw_f = gyro_report.z_raw;
float xraw_f = swap16(raw_gyro_report.x);
float yraw_f = swap16(raw_gyro_report.y);
float zraw_f = swap16(raw_gyro_report.z);

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

gyro_report.x_raw = (int16_t)(xraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
gyro_report.y_raw = (int16_t)(yraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
gyro_report.z_raw = (int16_t)(zraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]

float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
Expand Down
28 changes: 14 additions & 14 deletions src/drivers/fxos8701cq/fxos8701cq.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1405,23 +1405,23 @@ FXOS8701CQ::measure()
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);

accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x);
accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);

/* Save off the Mag readings todo: revist integrating theses */

_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
_last_raw_mag_y = swap16(raw_accel_mag_report.my);
_last_raw_mag_z = swap16(raw_accel_mag_report.mz);

float xraw_f = accel_report.x_raw;
float yraw_f = accel_report.y_raw;
float zraw_f = accel_report.z_raw;
float xraw_f = swap16RightJustify14(raw_accel_mag_report.x);
float yraw_f = swap16RightJustify14(raw_accel_mag_report.y);
float zraw_f = swap16RightJustify14(raw_accel_mag_report.z);

// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

accel_report.x_raw = (int16_t)(xraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
accel_report.y_raw = (int16_t)(yraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]
accel_report.z_raw = (int16_t)(zraw_f * _accel_range_scale * 1000); // (int16) [m / s^2 * 1000]

float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
Expand Down Expand Up @@ -1522,17 +1522,17 @@ FXOS8701CQ::mag_measure()
mag_report.timestamp = hrt_absolute_time();
mag_report.is_external = external();

mag_report.x_raw = _last_raw_mag_x;
mag_report.y_raw = _last_raw_mag_y;
mag_report.z_raw = _last_raw_mag_z;

float xraw_f = mag_report.x_raw;
float yraw_f = mag_report.y_raw;
float zraw_f = mag_report.z_raw;
float xraw_f = _last_raw_mag_x;
float yraw_f = _last_raw_mag_y;
float zraw_f = _last_raw_mag_z;

/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

mag_report.x_raw = (int16_t)(xraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]
mag_report.y_raw = (int16_t)(yraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]
mag_report.z_raw = (int16_t)(zraw_f * _mag_range_scale * 1000); // (int16) [Gs * 1000]

mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
Expand Down
15 changes: 4 additions & 11 deletions src/drivers/hmc5883/hmc5883.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -971,17 +971,6 @@ HMC5883::collect()
}
}

/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = -report.y;
new_report.y_raw = report.x;
/* z remains z */
new_report.z_raw = report.z;

/* scale values for output */

// XXX revisit for SPI part, might require a bus type IOCTL
Expand All @@ -1006,6 +995,10 @@ HMC5883::collect()
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

new_report.x_raw = (int16_t)(xraw_f * _range_scale * 1000); // (int16) [Gs * 1000]
new_report.y_raw = (int16_t)(yraw_f * _range_scale * 1000); // (int16) [Gs * 1000]
new_report.z_raw = (int16_t)(zraw_f * _range_scale * 1000); // (int16) [Gs * 1000]

new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
Expand Down
15 changes: 5 additions & 10 deletions src/drivers/ist8310/ist8310.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -950,23 +950,18 @@ IST8310::collect()
/* temperature measurement is not available on IST8310 */
new_report.temperature = 0;

/*
* raw outputs
*
* Sensor doesn't follow right hand rule, swap x and y to make it obey
* it.
*/
new_report.x_raw = report.y;
new_report.y_raw = report.x;
new_report.z_raw = report.z;

/* scale values for output */
xraw_f = report.y;
yraw_f = report.x;
zraw_f = report.z;

/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

new_report.x_raw = (int16_t)(xraw_f * _range_scale * 1000); // (int16) [Gs * 1000]
new_report.y_raw = (int16_t)(yraw_f * _range_scale * 1000); // (int16) [Gs * 1000]
new_report.z_raw = (int16_t)(zraw_f * _range_scale * 1000); // (int16) [Gs * 1000]

new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
Expand Down
4 changes: 4 additions & 0 deletions src/drivers/l3gd20/l3gd20.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1024,6 +1024,10 @@ L3GD20::measure()
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

report.x_raw = (int16_t)(xraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
report.y_raw = (int16_t)(yraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]
report.z_raw = (int16_t)(zraw_f * _gyro_range_scale * 1000); // (int16) [rad / s * 1000]

float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
Expand Down
Loading