52 #define MPU9250_TIMER_REDUCTION 200 55 #define ACCEL_RANGE_G 16 74 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(interface->get_device_id())),
75 _interface(interface),
78 _mag(this, mag_interface, rotation),
120 PX4_DEBUG(
"probe failed");
127 PX4_ERR(
"Exiting! Device failed to take initialization");
138 PX4_ERR(
"failed to setup ak8963 interface");
146 PX4_DEBUG(
"mag reset failed");
245 while (!all_ok && retries--) {
261 return all_ok ?
OK : -EIO;
285 PX4_DEBUG(
"unexpected whoami 0x%02x",
_whoami);
299 if (desired_sample_rate_hz == 0) {
306 div = 1000 / desired_sample_rate_hz;
310 if (div > 200) { div = 200; }
312 if (div < 1) { div = 1; }
339 if (frequency_hz == 0) {
343 }
else if (frequency_hz <= 5) {
347 }
else if (frequency_hz <= 10) {
351 }
else if (frequency_hz <= 20) {
355 }
else if (frequency_hz <= 41) {
359 }
else if (frequency_hz <= 92) {
363 }
else if (frequency_hz <= 184) {
367 }
else if (frequency_hz <= 250) {
409 return (uint16_t)(buf[0] << 8) | buf[1];
463 }
else if (max_g_in > 4) {
468 }
else if (max_g_in > 2) {
714 int16_t accel_xt = report.accel_y;
715 int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
717 int16_t gyro_xt = report.gyro_y;
718 int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
721 report.accel_x = accel_xt;
722 report.accel_y = accel_yt;
723 report.gyro_x = gyro_xt;
724 report.gyro_y = gyro_yt;
735 _px4_accel.
update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
736 _px4_gyro.
update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the mpu.
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the mpu, updating _checked_values.
#define BIT_INT_ANYRD_2CLEAR
#define BITS_DLPF_CFG_20HZ
#define BIT_INT_BYPASS_EN
virtual int init()
Initialise the driver and make it ready for use.
#define MPUREG_INT_STATUS
#define BITS_DLPF_CFG_92HZ
#define BITS_DLPF_CFG_250HZ
perf_counter_t _good_transfers
void _measure(hrt_abstime timestamp_sample, ak8963_regs data)
measure the time elapsed performing an event
#define BITS_DLPF_CFG_3600HZ
device::Device * _interface
#define BITS_ACCEL_CONFIG2_41HZ
#define MPU9250_HIGH_BUS_SPEED
int reset_mpu()
Resets the main chip (excluding the magnetometer if any).
device::Device * _interface
void update(hrt_abstime timestamp, float x, float y, float z)
uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED)
Read a register from the mpu.
#define BITS_DLPF_CFG_10HZ
uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]
MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation)
int set_accel_range(unsigned max_g)
Set the mpu measurement range.
#define DRV_GYR_DEVTYPE_MPU9250
void print_info()
Diagnostics - print some basic information about the driver.
virtual int probe()
whoami result
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a checked register in the mpu.
count the number of times an event occurs
#define BITS_DLPF_CFG_41HZ
uint16_t read_reg16(unsigned reg)
const uint16_t * _checked_registers
void set_device_type(uint8_t devtype)
static constexpr float CONSTANTS_ONE_G
void set_error_count(uint64_t error_count)
#define MPUREG_ACCEL_CONFIG
#define MPU9250_TIMER_REDUCTION
uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
Read a register range from the mpu.
#define MPUREG_SMPLRT_DIV
#define MPU9250_SET_SPEED(r, s)
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
Report conversation within the mpu, including command byte and interrupt status.
void perf_free(perf_counter_t handle)
Free a counter.
#define MPUREG_INT_ENABLE
void set_temperature(float temperature)
#define MPUREG_INT_PIN_CFG
#define MPU9250_GYRO_DEFAULT_RATE
#define MPUREG_PWR_MGMT_1
perf_counter_t _bad_transfers
Rotation
Enum for board and external compass rotations.
static struct mag_report _mag
mag report
uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
unsigned _num_checked_registers
void set_error_count(uint64_t error_count)
bool check_duplicate(uint8_t *accel_data)
void perf_end(perf_counter_t handle)
End a performance event.
static constexpr int MPU9250_NUM_CHECKED_REGISTERS
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
bool check_null_data(uint16_t *data, uint8_t size)
void set_scale(float scale)
uint8_t _last_accel_data[6]
void set_temperature(float temperature)
#define MPUREG_PWR_MGMT_2
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ
void measure()
Fetch measurements from the sensor and update the report buffers.
perf_counter_t _duplicates
#define MPUREG_ACCEL_CONFIG2
#define BITS_DLPF_CFG_5HZ
perf_counter_t _bad_registers
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.
Fundamental base class for all physical drivers (I2C, SPI).
#define MPUREG_GYRO_CONFIG
void _set_dlpf_filter(uint16_t frequency_hz)
void write_reg(unsigned reg, uint8_t value)
Write a register in the mpu.
int16_t int16_t_from_bytes(uint8_t bytes[])
Converts a signed 16 bit integer from big endian to little endian.
PX4Accelerometer _px4_accel
void set_scale(float scale)
DeviceBusType get_device_bus_type() const
Return the bus type the device is connected to.
#define MPU9250_LOW_SPEED_OP(r)
void set_device_type(uint8_t devtype)
static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]
void _set_sample_rate(unsigned desired_sample_rate_hz)
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t _sample_perf
#define DRV_ACC_DEVTYPE_MPU9250
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
#define BITS_DLPF_CFG_184HZ