37 #define DIR_READ(a) ((a) | (1 << 7)) 38 #define DIR_WRITE(a) ((a) & 0x7f) 39 #define swap16(w) __builtin_bswap16((w)) 41 #define FXAS21002C_STATUS 0x00 42 #define FXAS21002C_OUT_X_MSB 0x01 43 #define FXAS21002C_OUT_X_LSB 0x02 44 #define FXAS21002C_OUT_Y_MSB 0x03 45 #define FXAS21002C_OUT_Y_LSB 0x04 46 #define FXAS21002C_OUT_Z_MSB 0x05 47 #define FXAS21002C_OUT_Z_LSB 0x06 49 #define FXAS21002C_DR_STATUS 0x07 50 # define DR_STATUS_ZYXOW (1 << 7) 51 # define DR_STATUS_ZOW (1 << 6) 52 # define DR_STATUS_YOW (1 << 5) 53 # define DR_STATUS_XOW (1 << 4) 54 # define DR_STATUS_ZYXDR (1 << 3) 55 # define DR_STATUS_ZDR (1 << 2) 56 # define DR_STATUS_YDR (1 << 1) 57 # define DR_STATUS_XDR (1 << 0) 59 #define FXAS21002C_F_STATUS 0x08 60 # define F_STATUS_F_OVF (1 << 7) 61 # define F_STATUS_F_WMKF (1 << 6) 62 # define F_STATUS_F_CNT_SHIFTS 0 63 # define F_STATUS_F_CNT_MASK (0x3f << F_STATUS_F_CNT_SHIFTS) 65 #define FXAS21002C_F_SETUP 0x09 66 # define F_SETUP_F_MODE_SHIFTS 6 67 # define F_SETUP_F_MODE_MASK (0x3 << F_SETUP_F_MODE_SHIFTS) 68 # define F_SETUP_F_WMRK_SHIFTS 0 69 # define F_SETUP_F_WMRK_MASK (0x3f << F_SETUP_F_WMRK_SHIFTS) 71 #define FXAS21002C_F_EVENT 0x0a 72 # define F_EVENT_F_EVENT (1 << 5) 73 # define F_EVENT_FE_TIME_SHIFTS 0 74 # define F_EVENT_FE_TIME_MASK (0x1f << F_EVENT_FE_TIME_SHIFTS) 76 #define FXAS21002C_INT_SRC_FLAG 0x0b 77 # define INT_SRC_FLAG_BOOTEND (1 << 3) 78 # define INT_SRC_FLAG_SRC_FIFO (1 << 2) 79 # define INT_SRC_FLAG_SRC_RT (1 << 1) 80 # define INT_SRC_FLAG_SRC_DRDY (1 << 0) 82 #define FXAS21002C_WHO_AM_I 0x0c 85 #define FXAS21002C_CTRL_REG0 0x0d 86 # define CTRL_REG0_BW_SHIFTS 6 87 # define CTRL_REG0_BW_MASK (0x3 << CTRL_REG0_BW_SHIFTS) 88 # define CTRL_REG0_BW(n) (((n) & 0x3) << CTRL_REG0_BW_SHIFTS) 89 # define CTRL_REG0_BW_HIGH CTRL_REG0_BW(0) 90 # define CTRL_REG0_BW_MED CTRL_REG0_BW(1) 91 # define CTRL_REG0_BW_LOW CTRL_REG0_BW(2) 92 # define CTRL_REG0_SPIW (1 << 6) 93 # define CTRL_REG0_SEL_SHIFTS 3 94 # define CTRL_REG0_SEL_MASK (0x2 << CTRL_REG0_SEL_SHIFTS) 95 # define CTRL_REG0_HPF_EN (1 << 2) 96 # define CTRL_REG0_FS_SHIFTS 0 97 # define CTRL_REG0_FS_MASK (0x3 << CTRL_REG0_FS_SHIFTS) 98 # define CTRL_REG0_FS_2000_DPS (0 << CTRL_REG0_FS_SHIFTS) 99 # define CTRL_REG0_FS_1000_DPS (1 << CTRL_REG0_FS_SHIFTS) 100 # define CTRL_REG0_FS_500_DPS (2 << CTRL_REG0_FS_SHIFTS) 101 # define CTRL_REG0_FS_250_DPS (3 << CTRL_REG0_FS_SHIFTS) 103 #define FXAS21002C_RT_CFG 0x0e 104 # define RT_CFG_ELE (1 << 3) 105 # define RT_CFG_ZTEFE (1 << 2) 106 # define RT_CFG_YTEFE (1 << 1) 107 # define RT_CFG_XTEFE (1 << 0) 109 #define FXAS21002C_RT_SRC 0x0f 110 # define RT_SRC_EA (1 << 6) 111 # define RT_SRC_ZRT (1 << 5) 112 # define RT_SRC_Z_RT_POL (1 << 4) 113 # define RT_SRC_YRT (1 << 3) 114 # define RT_SRC_Y_RT_POL (1 << 2) 115 # define RT_SRC_XRT (1 << 1) 116 # define RT_SRC_X_RT_POL (1 << 0) 118 #define FXAS21002C_RT_THS 0x10 119 # define RT_THS_DBCNTM (1 << 7) 120 # define RT_THS_THS_SHIFTS 0 121 # define RT_THS_THS_MASK (0x7f << RT_THS_THS_SHIFTS) 123 #define FXAS21002C_RT_COUNT 0x11 124 #define FXAS21002C_TEMP 0x12 126 #define FXAS21002C_CTRL_REG1 0x13 127 # define CTRL_REG1_RST (1 << 6) 128 # define CTRL_REG1_ST (1 << 5) 129 # define CTRL_REG1_DR_SHIFTS 2 130 # define CTRL_REG1_DR_MASK (0x07 << CTRL_REG1_DR_SHIFTS) 131 # define CTRL_REG1_DR_12_5 (7 << CTRL_REG1_DR_SHIFTS) 132 # define CTRL_REG1_DR_12_5_1 (6 << CTRL_REG1_DR_SHIFTS) 133 # define CTRL_REG1_DR_25HZ (5 << CTRL_REG1_DR_SHIFTS) 134 # define CTRL_REG1_DR_50HZ (4 << CTRL_REG1_DR_SHIFTS) 135 # define CTRL_REG1_DR_100HZ (3 << CTRL_REG1_DR_SHIFTS) 136 # define CTRL_REG1_DR_200HZ (2 << CTRL_REG1_DR_SHIFTS) 137 # define CTRL_REG1_DR_400HZ (1 << CTRL_REG1_DR_SHIFTS) 138 # define CTRL_REG1_DR_800HZ (0 << CTRL_REG1_DR_SHIFTS) 139 # define CTRL_REG1_ACTIVE (1 << 1) 140 # define CTRL_REG1_READY (1 << 0) 142 #define FXAS21002C_CTRL_REG2 0x14 143 # define CTRL_REG2_INT_CFG_FIFO (1 << 7) 144 # define CTRL_REG2_INT_EN_FIFO (1 << 6) 145 # define CTRL_REG2_INT_CFG_RT (1 << 5) 146 # define CTRL_REG2_INT_EN_RT (1 << 4) 147 # define CTRL_REG2_INT_CFG_DRDY (1 << 3) 148 # define CTRL_REG2_INT_EN_DRDY (1 << 2) 149 # define CTRL_REG2_IPOL (1 << 1) 150 # define CTRL_REG2_PP_OD (1 << 0) 152 #define FXAS21002C_CTRL_REG3 0x15 153 # define CTRL_REG3_WRAPTOONE (1 << 3) 154 # define CTRL_REG3_EXTCTRLEN (1 << 2) 155 # define CTRL_REG3_FS_DOUBLE (1 << 0) 157 #define DEF_REG(r) {r, #r} 160 #define FXAS21002C_MAX_RATE 800 161 #define FXAS21002C_DEFAULT_RATE FXAS21002C_MAX_RATE 162 #define FXAS21002C_DEFAULT_RANGE_DPS 2000 163 #define FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ 64 // ODR dependant 172 #define FXAS21002C_TIMER_REDUCTION 250 190 SPI(
"FXAS21002C", nullptr, bus, device,
SPIDEV_MODE0, 2 * 1000 * 1000),
191 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(this->get_device_id())),
218 PX4_ERR(
"SPI init failed");
282 transfer(cmd, cmd,
sizeof(cmd));
295 transfer(cmd,
nullptr,
sizeof(cmd));
323 float new_range_scale_dps_digit;
329 if (max_dps <= 250) {
331 new_range_scale_dps_digit = 7.8125e-3
f;
334 }
else if (max_dps <= 500) {
336 new_range_scale_dps_digit = 15.625e-3
f;
339 }
else if (max_dps <= 1000) {
341 new_range_scale_dps_digit = 31.25e-3
f;
344 }
else if (max_dps <= 2000) {
346 new_range_scale_dps_digit = 62.5e-3
f;
380 int wait_ms = (1000 / rate) + 60 + 1;
382 usleep(wait_ms * 1000);
392 if (frequency == 0) {
396 if (frequency <= 13) {
400 }
else if (frequency <= 25) {
404 }
else if (frequency <= 50) {
408 }
else if (frequency <= 100) {
412 }
else if (frequency <= 200) {
416 }
else if (frequency <= 400) {
420 }
else if (frequency <= 800) {
455 if (frequency_hz == 0) {
458 }
else if (frequency_hz <= low) {
461 }
else if (frequency_hz <= med) {
464 }
else if (frequency_hz <= high) {
516 if (_checked_next != 0) {
533 #pragma pack(push, 1) 556 transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report,
sizeof(raw_gyro_report));
581 int16_t x_raw =
swap16(raw_gyro_report.x);
582 int16_t y_raw =
swap16(raw_gyro_report.y);
583 int16_t z_raw =
swap16(raw_gyro_report.z);
596 printf(
"gyro reads: %u\n",
_read);
607 ::printf(
"reg %02x:%02x should be %02x\n",
648 for (uint8_t i = 0; i <
sizeof(regmap) /
sizeof(regmap[0]); i++) {
649 printf(
"0x%02x %d:%s\n",
read_reg(regmap[i].reg), regmap[i].reg, regmap[i].
name);
FXAS21002C(int bus, uint32_t device, enum Rotation rotation)
#define FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ
#define FXAS21002C_OUT_X_LSB
static struct vehicle_status_s status
#define FXAS21002C_DEFAULT_RATE
void test_error()
deliberately trigger an error
measure the time elapsed performing an event
#define CTRL_REG1_DR_800HZ
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the FXAS21002C.
void start()
Start automatic measurement.
void set_standby(int rate, bool standby_true)
Put the chip In stand by.
void print_registers()
dump register values
#define CTRL_REG0_FS_MASK
#define CTRL_REG0_FS_1000_DPS
#define FXAS21002C_CTRL_REG0
#define FXAS21002C_STATUS
#define CTRL_REG1_DR_25HZ
#define CTRL_REG1_DR_MASK
#define FXAS21002C_F_SETUP
#define FXAS21002C_RT_COUNT
void check_registers(void)
check key registers for correct values
#define FXAS21002C_DEFAULT_RANGE_DPS
Namespace encapsulating all device framework classes, functions and data.
count the number of times an event occurs
void write_reg(unsigned reg, uint8_t value)
Write a register in the FXAS21002C.
#define CTRL_REG0_FS_250_DPS
void set_device_type(uint8_t devtype)
#define FXAS21002C_OUT_Y_LSB
#define FXAS21002C_DR_STATUS
#define FXAS21002C_OUT_X_MSB
#define FXAS21002C_CTRL_REG2
#define FXAS21002C_OUT_Z_LSB
#define FXAS21002C_RT_THS
#define FXAS21002C_WHO_AM_I
perf_counter_t _sample_perf
void update(hrt_abstime timestamp, float x, float y, float z)
uint8_t read_reg(unsigned reg)
Read a register from the FXAS21002C.
void stop()
Stop automatic measurement.
#define CTRL_REG1_DR_12_5
void perf_count(perf_counter_t handle)
Count a performance event.
#define FXAS21002C_OUT_Z_MSB
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void set_sample_rate(unsigned rate)
Rotation
Enum for board and external compass rotations.
perf_counter_t _bad_registers
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void set_onchip_lowpass_filter(int frequency_hz)
#define CTRL_REG0_BW_HIGH
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
#define CTRL_REG0_BW_MASK
int set_range(unsigned max_dps)
Set the FXAS21002C measurement range.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
#define DRV_GYR_DEVTYPE_FXAS2100C
Driver for the NXP FXAS21002C 3-Axis Digital Angular Rate Gyroscope connected via SPI...
#define FXAS21002C_TIMER_REDUCTION
#define CTRL_REG1_DR_100HZ
void measure()
Fetch accel measurements from the sensor and update the report ring.
void set_temperature(float temperature)
uint8_t _checked_values[FXAS21002C_NUM_CHECKED_REGISTERS]
#define CTRL_REG1_DR_50HZ
int set_samplerate(unsigned frequency)
Set the FXAS21002C internal sampling frequency.
#define FXAS21002C_CTRL_REG3
#define CTRL_REG0_FS_500_DPS
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.
#define FXAS21002C_CTRL_REG1
void print_info()
Diagnostics - print some basic information about the driver.
#define FXAS21002C_RT_CFG
#define FXAS21002C_OUT_Y_MSB
#define FXAS21002C_RT_SRC
void set_scale(float scale)
#define FXAS21002C_INT_SRC_FLAG
perf_counter_t _duplicates
#define FXAS21002C_F_EVENT
#define CTRL_REG1_DR_200HZ
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define FXAS21002C_F_STATUS
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the FXAS21002C, updating _checked_values.
static constexpr int FXAS21002C_NUM_CHECKED_REGISTERS
#define CTRL_REG1_DR_400HZ
#define CTRL_REG0_FS_2000_DPS
static constexpr uint8_t _checked_registers[]