fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 08:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up scaling / offset handling for mag and gyro / acc

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-26 14:51:33 +02:00
parent 6026595d83
commit 207b077d8a
2 changed files with 29 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/hmc5883/hmc5883.cpp
+26 -6
View File
@@ -143,6 +143,8 @@ private:
volatile unsigned _oldest_report;
mag_report *_reports;
mag_scale _scale;
float _range_scale;
float _range_ga;
bool _collect_phase;
orb_advert_t _mag_topic;
@@ -253,6 +255,8 @@ HMC5883::HMC5883(int bus) :
_oldest_report(0),
_reports(nullptr),
_mag_topic(-1),
_range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */
_range_ga(0.88f),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
@@ -262,11 +266,11 @@ HMC5883::HMC5883(int bus) :
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */
_scale.z_scale = 1.0f;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@@ -666,11 +670,27 @@ HMC5883::collect()
_reports[_next_report].z_raw = report.z;
/* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* to align the sensor axes with the board, x and y need to be flipped */
_reports[_next_report].x = report.y * _scale.x_scale + _scale.x_offset;
_reports[_next_report].y = report.x * _scale.y_scale + _scale.y_offset;
_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset;
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
apps/drivers/ms5611/ms5611.cpp
+3 -3
View File
@@ -834,9 +834,9 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("pressure: %u", (unsigned)report.pressure);
warnx("altitude: %u", (unsigned)report.altitude);
warnx("temperature: %u", (unsigned)report.temperature);
warnx("pressure: %10.4f", report.pressure);
warnx("altitude: %11.4f", report.altitude);
warnx("temperature: %8.4f", report.temperature);
warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */