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

LSM303D accel raw values look ok (work in progress)

Browse Source
This commit is contained in:
Julian Oes
2013-04-06 11:36:38 +02:00
committed by Lorenz Meier
parent b4483a09b2
commit 2187dc8e9a
1 changed files with 206 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/lsm303d/lsm303d.cpp
+206 -199
View File
@@ -83,10 +83,31 @@ static const int ERROR = -1;
#define ADDR_STATUS_M 0x07 #define ADDR_STATUS_M 0x07
#define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_L_M 0x08
#define ADDR_OUT_X_H_M 0x09 #define ADDR_OUT_X_H_M 0x09
#define ADDR_OUT_Y_L_M 0x08 #define ADDR_OUT_Y_L_M 0x0A
#define ADDR_OUT_Y_H_M 0x09 #define ADDR_OUT_Y_H_M 0x0B
#define ADDR_OUT_Z_L_M 0x0A #define ADDR_OUT_Z_L_M 0x0C
#define ADDR_OUT_Z_H_M 0x0B #define ADDR_OUT_Z_H_M 0x0D
#define ADDR_OUT_TEMP_A 0x26
#define ADDR_STATUS_A 0x27
#define ADDR_OUT_X_L_A 0x28
#define ADDR_OUT_X_H_A 0x29
#define ADDR_OUT_Y_L_A 0x2A
#define ADDR_OUT_Y_H_A 0x2B
#define ADDR_OUT_Z_L_A 0x2C
#define ADDR_OUT_Z_H_A 0x2D
#define ADDR_CTRL_REG1 0x20
#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
#define REG1_CONT_UPDATE_A (0<<3)
#define REG1_Z_ENABLE_A (1<<2)
#define REG1_Y_ENABLE_A (1<<1)
#define REG1_X_ENABLE_A (1<<0)
#define ADDR_WHO_AM_I 0x0F #define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM 0x49 #define WHO_I_AM 0x49
@@ -274,7 +295,7 @@ LSM303D::init()
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
// /* set default configuration */ // /* set default configuration */
// write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG1, REG1_RATE_100HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
// write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ // write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
// write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ // write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
// write_reg(ADDR_CTRL_REG4, REG4_BDU); // write_reg(ADDR_CTRL_REG4, REG4_BDU);
@@ -310,37 +331,37 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
unsigned count = buflen / sizeof(struct accel_report); unsigned count = buflen / sizeof(struct accel_report);
int ret = 0; int ret = 0;
// /* buffer must be large enough */ /* buffer must be large enough */
// if (count < 1) if (count < 1)
// return -ENOSPC; return -ENOSPC;
//
// /* if automatic measurement is enabled */ /* if automatic measurement is enabled */
// if (_call_interval > 0) { if (_call_interval > 0) {
//
// /* /*
// * While there is space in the caller's buffer, and reports, copy them. * While there is space in the caller's buffer, and reports, copy them.
// * Note that we may be pre-empted by the measurement code while we are doing this; * Note that we may be pre-empted by the measurement code while we are doing this;
// * we are careful to avoid racing with it. * we are careful to avoid racing with it.
// */ */
// while (count--) { while (count--) {
// if (_oldest_report != _next_report) { if (_oldest_report != _next_report) {
// memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
// ret += sizeof(_reports[0]); ret += sizeof(_reports[0]);
// INCREMENT(_oldest_report, _num_reports); INCREMENT(_oldest_report, _num_reports);
// } }
// } }
//
// /* if there was no data, warn the caller */ /* if there was no data, warn the caller */
// return ret ? ret : -EAGAIN; return ret ? ret : -EAGAIN;
// } }
//
// /* manual measurement */ /* manual measurement */
// _oldest_report = _next_report = 0; _oldest_report = _next_report = 0;
// measure(); measure();
//
// /* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
// memcpy(buffer, _reports, sizeof(*_reports)); memcpy(buffer, _reports, sizeof(*_reports));
// ret = sizeof(*_reports); ret = sizeof(*_reports);
return ret; return ret;
} }
@@ -350,116 +371,89 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
{ {
switch (cmd) { switch (cmd) {
// case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
// switch (arg) { switch (arg) {
//
// /* switching to manual polling */ /* switching to manual polling */
// case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
// stop(); stop();
// _call_interval = 0; _call_interval = 0;
// return OK; return OK;
//
// /* external signalling not supported */ /* external signalling not supported */
// case SENSOR_POLLRATE_EXTERNAL: case SENSOR_POLLRATE_EXTERNAL:
//
// /* zero would be bad */ /* zero would be bad */
// case 0: case 0:
// return -EINVAL; return -EINVAL;
//
// /* set default/max polling rate */ /* set default/max polling rate */
// case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
// case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
// /* With internal low pass filters enabled, 250 Hz is sufficient */ /* With internal low pass filters enabled, 250 Hz is sufficient */
// return ioctl(filp, SENSORIOCSPOLLRATE, 250); return ioctl(filp, SENSORIOCSPOLLRATE, 250);
//
// /* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
// default: { default: {
// /* do we need to start internal polling? */ /* do we need to start internal polling? */
// bool want_start = (_call_interval == 0); bool want_start = (_call_interval == 0);
//
// /* convert hz to hrt interval via microseconds */ /* convert hz to hrt interval via microseconds */
// unsigned ticks = 1000000 / arg; unsigned ticks = 1000000 / arg;
//
// /* check against maximum sane rate */ /* check against maximum sane rate */
// if (ticks < 1000) if (ticks < 1000)
// return -EINVAL; return -EINVAL;
//
// /* update interval for next measurement */ /* update interval for next measurement */
// /* XXX this is a bit shady, but no other way to adjust... */ /* XXX this is a bit shady, but no other way to adjust... */
// _call.period = _call_interval = ticks; _call.period = _call_interval = ticks;
//
// /* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
// if (want_start) if (want_start)
// start(); start();
//
// return OK; return OK;
// } }
// } }
// } }
//
// case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
// if (_call_interval == 0) if (_call_interval == 0)
// return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
//
// return 1000000 / _call_interval; return 1000000 / _call_interval;
//
// case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
// /* account for sentinel in the ring */ /* account for sentinel in the ring */
// arg++; arg++;
//
// /* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
// if ((arg < 2) || (arg > 100)) if ((arg < 2) || (arg > 100))
// return -EINVAL; return -EINVAL;
//
// /* allocate new buffer */ /* allocate new buffer */
// struct accel_report *buf = new struct accel_report[arg]; struct accel_report *buf = new struct accel_report[arg];
//
// if (nullptr == buf) if (nullptr == buf)
// return -ENOMEM; return -ENOMEM;
//
// /* reset the measurement state machine with the new buffer, free the old */ /* reset the measurement state machine with the new buffer, free the old */
// stop(); stop();
// delete[] _reports; delete[] _reports;
// _num_reports = arg; _num_reports = arg;
// _reports = buf; _reports = buf;
// start(); start();
//
// return OK; return OK;
// } }
//
// case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
// return _num_reports - 1; return _num_reports - 1;
//
// case SENSORIOCRESET: case SENSORIOCRESET:
// /* XXX implement */ /* XXX implement */
// return -EINVAL; return -EINVAL;
//
// case GYROIOCSSAMPLERATE:
// return set_samplerate(arg);
//
// case GYROIOCGSAMPLERATE:
// return _current_rate;
//
// case GYROIOCSLOWPASS:
// case GYROIOCGLOWPASS:
// /* XXX not implemented due to wacky interaction with samplerate */
// return -EINVAL;
//
// case GYROIOCSSCALE:
// /* copy scale in */
// memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
// return OK;
//
// case GYROIOCGSCALE:
// /* copy scale out */
// memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
// return OK;
//
// case GYROIOCSRANGE:
// return set_range(arg);
//
// case GYROIOCGRANGE:
// return _current_range;
default: default:
/* give it to the superclass */ /* give it to the superclass */
@@ -596,65 +590,78 @@ LSM303D::measure_trampoline(void *arg)
void void
LSM303D::measure() LSM303D::measure()
{ {
// /* status register and data as read back from the device */ /* status register and data as read back from the device */
//#pragma pack(push, 1) //#pragma pack(push, 1)
// struct { // struct {
// uint8_t cmd; // uint8_t cmd;
// uint8_t temp; // uint16_t temp;
// uint8_t status; // uint8_t status;
// int16_t x; // int16_t x;
// int16_t y; // int16_t y;
// int16_t z; // int16_t z;
// } raw_report; // } raw_report_mag;
//#pragma pack(pop) //#pragma pack(pop)
//
// accel_report *report = &_reports[_next_report]; #pragma pack(push, 1)
// struct {
// /* start the performance counter */ uint8_t cmd;
// perf_begin(_sample_perf); uint8_t status;
// int16_t x;
// /* fetch data from the sensor */ int16_t y;
// raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; int16_t z;
// transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); } raw_report_accel;
// #pragma pack(pop)
// /*
// * 1) Scale raw value to SI units using scaling from datasheet. accel_report *report = &_reports[_next_report];
// * 2) Subtract static offset (in SI units)
// * 3) Scale the statically calibrated values with a linear /* start the performance counter */
// * dynamically obtained factor perf_begin(_sample_perf);
// *
// * Note: the static sensor offset is the number the sensor outputs /* fetch data from the sensor */
// * at a nominally 'zero' input. Therefore the offset has to raw_report_accel.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
// * be subtracted. transfer((uint8_t *)&raw_report_accel, (uint8_t *)&raw_report_accel, sizeof(raw_report_accel));
// *
// * Example: A gyro outputs a value of 74 at zero angular rate /*
// * the offset is 74 from the origin and subtracting * 1) Scale raw value to SI units using scaling from datasheet.
// * 74 from all measurements centers them around zero. * 2) Subtract static offset (in SI units)
// */ * 3) Scale the statically calibrated values with a linear
// report->timestamp = hrt_absolute_time(); * dynamically obtained factor
// /* XXX adjust for sensor alignment to board here */ *
// report->x_raw = raw_report.x; * Note: the static sensor offset is the number the sensor outputs
// report->y_raw = raw_report.y; * at a nominally 'zero' input. Therefore the offset has to
// report->z_raw = raw_report.z; * 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.
*/
report->timestamp = hrt_absolute_time();
/* XXX adjust for sensor alignment to board here */
report->x_raw = raw_report_accel.x;
report->y_raw = raw_report_accel.y;
report->z_raw = raw_report_accel.z;
// report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; // report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
// report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; // report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
// report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
// report->scaling = _gyro_range_scale; // report->scaling = _gyro_range_scale;
// report->range_rad_s = _gyro_range_rad_s; // report->range_rad_s = _gyro_range_rad_s;
//
// /* post a report to the ring - note, not locked */ /* post a report to the ring - note, not locked */
// INCREMENT(_next_report, _num_reports); INCREMENT(_next_report, _num_reports);
//
// /* if we are running up against the oldest report, fix it */ /* if we are running up against the oldest report, fix it */
// if (_next_report == _oldest_report) if (_next_report == _oldest_report)
// INCREMENT(_oldest_report, _num_reports); INCREMENT(_oldest_report, _num_reports);
//
// /* notify anyone waiting for data */ /* notify anyone waiting for data */
// poll_notify(POLLIN); poll_notify(POLLIN);
//
// /* publish for subscribers */ /* publish for subscribers */
// orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
/* stop the perf counter */ /* stop the perf counter */
perf_end(_sample_perf); perf_end(_sample_perf);
@@ -740,22 +747,22 @@ test()
err(1, "%s open failed", ACCEL_DEVICE_PATH); err(1, "%s open failed", ACCEL_DEVICE_PATH);
/* reset to manual polling */ /* reset to manual polling */
if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) // if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling"); // err(1, "reset to manual polling");
/* do a simple demand read */ /* do a simple demand read */
sz = read(fd_accel, &a_report, sizeof(a_report)); sz = read(fd_accel, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) if (sz != sizeof(a_report))
err(1, "immediate gyro read failed"); err(1, "immediate read failed");
warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); // warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); // warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); // warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel x: \t%d\traw", (int)a_report.x_raw);
warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw);
warnx("accel z: \t%d\traw", (int)a_report.z_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw);
warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); // warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
/* XXX add poll-rate tests here too */ /* XXX add poll-rate tests here too */