diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 22 | ||||
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 16 | ||||
| -rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 33 | ||||
| -rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 32 |
4 files changed, 66 insertions, 37 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20b..2b3520fc8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + 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; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8..768723640 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1029,9 +1029,16 @@ L3GD20::measure() report.temperature_raw = raw_report.temp; - report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xraw_f = report.x_raw; + float yraw_f = report.y_raw; + float zraw_f = report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.x = _gyro_filter_x.apply(report.x); report.y = _gyro_filter_y.apply(report.y); @@ -1039,9 +1046,6 @@ L3GD20::measure() report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; - // apply user specified rotation - rotate_3f(_rotation, report.x, report.y, report.z); - report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b5f01b942..84f7fb5c8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1517,9 +1517,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + 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; /* we have logs where the accelerometers get stuck at a fixed @@ -1555,9 +1562,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1623,16 +1627,21 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + 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; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); - _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea8577..eaf25a9f3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1711,17 +1711,21 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + 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); + + 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; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, arb.x, arb.y, arb.z); - arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1732,17 +1736,21 @@ MPU6000::measure() grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + 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); + + 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; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - // apply user specified rotation - rotate_3f(_rotation, grb.x, grb.y, grb.z); - grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; |