46 #include <px4_platform_common/getopt.h> 47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 49 #define L3GD20_DEVICE_PATH "/dev/l3gd20" 52 #define SENSOR_BOARD_ROTATION_000_DEG 0 53 #define SENSOR_BOARD_ROTATION_090_DEG 1 54 #define SENSOR_BOARD_ROTATION_180_DEG 2 55 #define SENSOR_BOARD_ROTATION_270_DEG 3 58 #define DIR_READ (1<<7) 59 #define DIR_WRITE (0<<7) 60 #define ADDR_INCREMENT (1<<6) 63 #define ADDR_WHO_AM_I 0x0F 64 #define WHO_I_AM_H 0xD7 66 #define WHO_I_AM_L3G4200D 0xD3 68 #define ADDR_CTRL_REG1 0x20 69 #define REG1_RATE_LP_MASK 0xF0 72 #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) 73 #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) 74 #define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) 75 #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) 76 #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) 77 #define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) 78 #define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) 79 #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) 80 #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) 81 #define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) 82 #define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) 83 #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) 85 #define ADDR_CTRL_REG2 0x21 86 #define ADDR_CTRL_REG3 0x22 87 #define ADDR_CTRL_REG4 0x23 88 #define REG4_RANGE_MASK 0x30 89 #define RANGE_250DPS (0<<4) 90 #define RANGE_500DPS (1<<4) 91 #define RANGE_2000DPS (3<<4) 93 #define ADDR_CTRL_REG5 0x24 94 #define ADDR_REFERENCE 0x25 95 #define ADDR_OUT_TEMP 0x26 96 #define ADDR_STATUS_REG 0x27 97 #define ADDR_OUT_X_L 0x28 98 #define ADDR_OUT_X_H 0x29 99 #define ADDR_OUT_Y_L 0x2A 100 #define ADDR_OUT_Y_H 0x2B 101 #define ADDR_OUT_Z_L 0x2C 102 #define ADDR_OUT_Z_H 0x2D 103 #define ADDR_FIFO_CTRL_REG 0x2E 104 #define ADDR_FIFO_SRC_REG 0x2F 105 #define ADDR_INT1_CFG 0x30 106 #define ADDR_INT1_SRC 0x31 107 #define ADDR_INT1_TSH_XH 0x32 108 #define ADDR_INT1_TSH_XL 0x33 109 #define ADDR_INT1_TSH_YH 0x34 110 #define ADDR_INT1_TSH_YL 0x35 111 #define ADDR_INT1_TSH_ZH 0x36 112 #define ADDR_INT1_TSH_ZL 0x37 113 #define ADDR_INT1_DURATION 0x38 114 #define ADDR_LOW_ODR 0x39 118 #define REG1_POWER_NORMAL (1<<3) 119 #define REG1_Z_ENABLE (1<<2) 120 #define REG1_Y_ENABLE (1<<1) 121 #define REG1_X_ENABLE (1<<0) 123 #define REG4_BDU (1<<7) 124 #define REG4_BLE (1<<6) 127 #define REG5_FIFO_ENABLE (1<<6) 128 #define REG5_REBOOT_MEMORY (1<<7) 130 #define STATUS_ZYXOR (1<<7) 131 #define STATUS_ZOR (1<<6) 132 #define STATUS_YOR (1<<5) 133 #define STATUS_XOR (1<<4) 134 #define STATUS_ZYXDA (1<<3) 135 #define STATUS_ZDA (1<<2) 136 #define STATUS_YDA (1<<1) 137 #define STATUS_XDA (1<<0) 139 #define FIFO_CTRL_BYPASS_MODE (0<<5) 140 #define FIFO_CTRL_FIFO_MODE (1<<5) 141 #define FIFO_CTRL_STREAM_MODE (1<<6) 142 #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) 143 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) 145 #define L3GD20_DEFAULT_RATE 760 146 #define L3G4200D_DEFAULT_RATE 800 147 #define L3GD20_MAX_OUTPUT_RATE 280 148 #define L3GD20_DEFAULT_RANGE_DPS 2000 149 #define L3GD20_DEFAULT_FILTER_FREQ 30 150 #define L3GD20_TEMP_OFFSET_CELSIUS 40 152 #define L3GD20_MAX_OFFSET 0.45f 154 #ifndef SENSOR_BOARD_ROTATION_DEFAULT 155 #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG 164 #define L3GD20_TIMER_REDUCTION 600 168 class L3GD20 :
public device::SPI,
public px4::ScheduledWorkItem
275 void write_reg(
unsigned reg, uint8_t value);
286 void modify_reg(
unsigned reg, uint8_t clearbits, uint8_t setbits);
324 SPI(
"L3GD20", path, bus, device,
SPIDEV_MODE3, 11 * 1000 * 1000),
325 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(this->get_device_id())),
369 bool success =
false;
405 transfer(cmd, cmd,
sizeof(cmd));
418 transfer(cmd,
nullptr,
sizeof(cmd));
447 float new_range_scale_dps_digit;
453 if (max_dps <= 250) {
456 new_range_scale_dps_digit = 8.75e-3
f;
458 }
else if (max_dps <= 500) {
461 new_range_scale_dps_digit = 17.5e-3
f;
463 }
else if (max_dps <= 2000) {
466 new_range_scale_dps_digit = 70e-3
f;
484 if (frequency == 0) {
492 if (frequency <= 100) {
496 }
else if (frequency <= 200) {
500 }
else if (frequency <= 400) {
504 }
else if (frequency <= 800) {
537 uint8_t retries = 10;
609 if (_checked_next != 0) {
623 #pragma pack(push, 1) 642 transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report,
sizeof(raw_report));
671 _px4_gyro.
update(timestamp_sample, raw_report.y, raw_report.x, raw_report.z);
676 int16_t x = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
677 int16_t y = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
685 int16_t x = raw_report.y;
686 int16_t y = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
696 _px4_gyro.
update(timestamp_sample, raw_report.x, raw_report.y, raw_report.z);
708 printf(
"gyro reads: %u\n",
_read);
720 ::printf(
"reg %02x:%02x should be %02x\n",
733 printf(
"L3GD20 registers\n");
735 for (uint8_t reg = 0; reg <= 0x40; reg++) {
737 printf(
"%02x:%02x ", (
unsigned)reg, (
unsigned)v);
739 if ((reg + 1) % 16 == 0) {
777 if (g_dev !=
nullptr) {
778 errx(0,
"already started");
783 #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO) 786 errx(0,
"External SPI not available");
793 if (g_dev ==
nullptr) {
797 if (
OK != g_dev->
init()) {
804 if (g_dev !=
nullptr) {
809 errx(1,
"driver start failed");
818 if (g_dev ==
nullptr) {
819 errx(1,
"driver not running\n");
822 printf(
"state @ %p\n", g_dev);
834 if (g_dev ==
nullptr) {
835 errx(1,
"driver not running");
838 printf(
"regdump @ %p\n", g_dev);
850 if (g_dev ==
nullptr) {
851 errx(1,
"driver not running");
854 printf(
"regdump @ %p\n", g_dev);
863 warnx(
"missing command: try 'start', 'info', 'testerror' or 'regdump'");
865 warnx(
" -X (external bus)");
866 warnx(
" -R rotation");
876 const char *myoptarg =
nullptr;
877 bool external_bus =
false;
880 while ((ch = px4_getopt(argc, argv,
"XR:", &myoptind, &myoptarg)) != EOF) {
887 rotation = (
enum Rotation)atoi(myoptarg);
896 if (myoptind >= argc) {
901 const char *verb = argv[myoptind];
907 if (!strcmp(verb,
"start")) {
914 if (!strcmp(verb,
"info")) {
921 if (!strcmp(verb,
"regdump")) {
928 if (!strcmp(verb,
"testerror")) {
932 PX4_ERR(
"unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
L3GD20 operator=(const L3GD20 &)
#define ADDR_FIFO_CTRL_REG
#define L3GD20_TIMER_REDUCTION
perf_counter_t _duplicates
static struct vehicle_status_s status
static constexpr int L3GD20_NUM_CHECKED_REGISTERS
measure the time elapsed performing an event
#define FIFO_CTRL_BYPASS_MODE
#define SENSOR_BOARD_ROTATION_DEFAULT
void regdump()
Dump the register information.
void usage(const char *reason)
Print the correct usage.
__EXPORT int l3gd20_main(int argc, char *argv[])
uint8_t read_reg(unsigned reg)
Read a register from the L3GD20.
#define SENSOR_BOARD_ROTATION_180_DEG
#define RATE_95HZ_LP_25HZ
#define RATE_380HZ_LP_50HZ
perf_counter_t _bad_registers
void measure()
Fetch measurements from the sensor and update the report ring.
Namespace encapsulating all device framework classes, functions and data.
count the number of times an event occurs
void set_device_type(uint8_t devtype)
void disable_i2c()
disable I2C on the chip
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the L3GD20.
#define RATE_760HZ_LP_50HZ
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
#define DRV_GYR_DEVTYPE_L3GD20
void start()
Start automatic measurement.
#define RATE_190HZ_LP_50HZ
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void usage()
Prints info about the driver argument usage.
#define SENSOR_BOARD_ROTATION_090_DEG
Rotation
Enum for board and external compass rotations.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void check_registers(void)
check key registers for correct values
#define L3G4200D_DEFAULT_RATE
Local functions in support of the shell command.
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
void info()
Print a little info about the driver.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
#define SENSOR_BOARD_ROTATION_000_DEG
#define L3GD20_DEFAULT_RANGE_DPS
void print_info()
Diagnostics - print some basic information about the driver.
#define SENSOR_BOARD_ROTATION_270_DEG
uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]
L3GD20(int bus, const char *path, uint32_t device, enum Rotation rotation)
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the L3GD20, updating _checked_values.
#define L3GD20_TEMP_OFFSET_CELSIUS
void set_temperature(float temperature)
#define REG1_POWER_NORMAL
#define L3GD20_DEVICE_PATH
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
void write_reg(unsigned reg, uint8_t value)
Write a register in the L3GD20.
int set_range(unsigned max_dps)
Set the L3GD20 measurement range.
static constexpr uint8_t _checked_registers[]
void stop()
Stop automatic measurement.
void test_error()
trigger an error
void start(bool external_bus, enum Rotation rotation)
Start the driver.
void set_scale(float scale)
void reset()
Reset the driver.
int set_samplerate(unsigned frequency)
Set the L3GD20 internal sampling frequency.
perf_counter_t _sample_perf
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define WHO_I_AM_L3G4200D
#define DEVICE_DEBUG(FMT,...)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.