dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added orientation support and detection to the L3GD20/H driver to support the different variants in use

This commit is contained in:
Lorenz Meier 2013-08-16 16:24:44 +02:00
parent 52d15ac7b1
commit 05e4c086ce
1 changed files with 51 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/drivers/l3gd20/l3gd20.cpp
View File

@ -71,6 +71,12 @@
#endif
static const int ERROR = -1;
/* Orientation on board */
#define SENSOR_BOARD_ROTATION_000_DEG 0
#define SENSOR_BOARD_ROTATION_090_DEG 1
#define SENSOR_BOARD_ROTATION_180_DEG 2
#define SENSOR_BOARD_ROTATION_270_DEG 3
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@ -186,6 +192,7 @@ private:
unsigned _current_rate;
unsigned _current_range;
unsigned _orientation;
perf_counter_t _sample_perf;
@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_topic(-1),
_current_rate(0),
_current_range(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_gyro_filter_x(250, 30),
_gyro_filter_y(250, 30),
@ -363,8 +371,23 @@ L3GD20::probe()
(void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H)
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
_orientation = SENSOR_BOARD_ROTATION_270_DEG;
#else
#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
#endif
return OK;
}
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
_orientation = SENSOR_BOARD_ROTATION_180_DEG;
return OK;
}
return -EIO;
}
@ -717,9 +740,33 @@ L3GD20::measure()
*/
report->timestamp = hrt_absolute_time();
/* swap x and y and negate y */
report->x_raw = raw_report.y;
report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report->x_raw = raw_report.x;
report->y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report->x_raw = raw_report.y;
report->y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report->x_raw = raw_report.y;
report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report->z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;