PX4 Firmware
PX4 Autopilot Software http://px4.io
|
Helper class implementing the magnetometer driver node. More...
#include <MPU9250_mag.h>
Public Member Functions | |
MPU9250_mag (MPU9250 *parent, device::Device *interface, enum Rotation rotation) | |
~MPU9250_mag () | |
void | set_passthrough (uint8_t reg, uint8_t size, uint8_t *out=NULL) |
void | passthrough_read (uint8_t reg, uint8_t *buf, uint8_t size) |
void | passthrough_write (uint8_t reg, uint8_t val) |
void | read_block (uint8_t reg, uint8_t *val, uint8_t count) |
int | ak8963_reset () |
int | ak8963_setup () |
int | ak8963_setup_master_i2c () |
bool | ak8963_check_id (uint8_t &id) |
bool | ak8963_read_adjustments () |
void | print_status () |
Protected Member Functions | |
void | measure () |
void | _measure (hrt_abstime timestamp_sample, ak8963_regs data) |
uint8_t | read_reg (unsigned reg) |
void | write_reg (unsigned reg, uint8_t value) |
bool | is_passthrough () |
Protected Attributes | |
device::Device * | _interface |
Private Attributes | |
PX4Magnetometer | _px4_mag |
MPU9250 * | _parent |
bool | _mag_reading_data {false} |
perf_counter_t | _mag_overruns |
perf_counter_t | _mag_overflows |
perf_counter_t | _mag_errors |
Friends | |
class | MPU9250 |
Helper class implementing the magnetometer driver node.
Definition at line 112 of file MPU9250_mag.h.
MPU9250_mag::MPU9250_mag | ( | MPU9250 * | parent, |
device::Device * | interface, | ||
enum Rotation | rotation | ||
) |
Definition at line 54 of file MPU9250_mag.cpp.
References _px4_mag, DRV_MAG_DEVTYPE_MPU9250, MPU9250_MAG_RANGE_GA, PX4Magnetometer::set_device_type(), and PX4Magnetometer::set_scale().
MPU9250_mag::~MPU9250_mag | ( | ) |
Definition at line 67 of file MPU9250_mag.cpp.
References _mag_errors, _mag_overflows, _mag_overruns, and perf_free().
|
protected |
Definition at line 118 of file MPU9250_mag.cpp.
References MPU9250::_last_temperature, _parent, _px4_mag, AK09916_ST1_DRDY, MPU9250::is_external(), PX4Magnetometer::set_external(), PX4Magnetometer::set_temperature(), ak8963_regs::st1, PX4Magnetometer::update(), ak8963_regs::x, ak8963_regs::y, and ak8963_regs::z.
Referenced by measure(), and MPU9250::measure().
bool MPU9250_mag::ak8963_check_id | ( | uint8_t & | id | ) |
Definition at line 196 of file MPU9250_mag.cpp.
References AK8963_DEVICE_ID, AK8963REG_WIA, and read_reg().
Referenced by ak8963_setup().
bool MPU9250_mag::ak8963_read_adjustments | ( | ) |
Definition at line 244 of file MPU9250_mag.cpp.
References _interface, _px4_mag, AK8963_16BIT_ADC, AK8963_FUZE_MODE, AK8963_POWERDOWN_MODE, AK8963REG_ASAX, AK8963REG_CNTL1, f(), passthrough_read(), device::Device::read(), PX4Magnetometer::set_sensitivity(), and write_reg().
Referenced by ak8963_setup().
int MPU9250_mag::ak8963_reset | ( | ) |
Definition at line 227 of file MPU9250_mag.cpp.
References AK8963_RESET, ak8963_setup(), AK8963REG_CNTL2, OK, and write_reg().
Referenced by MPU9250::init(), and MPU9250::reset().
int MPU9250_mag::ak8963_setup | ( | ) |
Definition at line 295 of file MPU9250_mag.cpp.
References _interface, _parent, MPU9250::_whoami, AK8963_16BIT_ADC, ak8963_check_id(), AK8963_CONTINUOUS_MODE2, ak8963_read_adjustments(), AK8963_RESET, ak8963_setup_master_i2c(), AK8963REG_CNTL1, AK8963REG_CNTL2, AK8963REG_ST1, BIT_I2C_MST_EN, BIT_I2C_MST_RST, MPU9250::modify_checked_reg(), MPU9250::modify_reg(), MPU_WHOAMI_9250, MPUREG_I2C_MST_CTRL, MPUREG_USER_CTRL, OK, set_passthrough(), write_reg(), and MPU9250::write_reg().
Referenced by ak8963_reset().
int MPU9250_mag::ak8963_setup_master_i2c | ( | ) |
Definition at line 276 of file MPU9250_mag.cpp.
References _interface, _parent, MPU9250::_whoami, BIT_I2C_MST_EN, BIT_I2C_MST_P_NSR, BIT_I2C_MST_WAIT_FOR_ES, BITS_I2C_MST_CLOCK_400HZ, MPU9250::modify_checked_reg(), MPU_WHOAMI_9250, MPUREG_I2C_MST_CTRL, MPUREG_USER_CTRL, OK, and MPU9250::write_reg().
Referenced by ak8963_setup().
|
inlineprotected |
Definition at line 142 of file MPU9250_mag.h.
Referenced by MPU9250::init(), MPU9250::measure(), and MPU9250::reset_mpu().
|
protected |
Definition at line 75 of file MPU9250_mag.cpp.
References _interface, _mag_errors, _mag_overflows, _mag_overruns, _measure(), _px4_mag, AK09916_ST1_DRDY, AK8963REG_ST1, data, hrt_absolute_time(), hrt_abstime, OK, perf_count(), perf_event_count(), device::Device::read(), and PX4Magnetometer::set_error_count().
Referenced by MPU9250::measure().
void MPU9250_mag::passthrough_read | ( | uint8_t | reg, |
uint8_t * | buf, | ||
uint8_t | size | ||
) |
Definition at line 172 of file MPU9250_mag.cpp.
References _parent, MPUREG_EXT_SENS_DATA_00, MPUREG_I2C_SLV0_CTRL, read_block(), set_passthrough(), and MPU9250::write_reg().
Referenced by ak8963_read_adjustments(), and read_reg().
void MPU9250_mag::passthrough_write | ( | uint8_t | reg, |
uint8_t | val | ||
) |
Definition at line 207 of file MPU9250_mag.cpp.
References _parent, MPUREG_I2C_SLV0_CTRL, set_passthrough(), and MPU9250::write_reg().
Referenced by write_reg().
|
inline |
Definition at line 129 of file MPU9250_mag.h.
Referenced by MPU9250::print_info().
void MPU9250_mag::read_block | ( | uint8_t | reg, |
uint8_t * | val, | ||
uint8_t | count | ||
) |
Definition at line 166 of file MPU9250_mag.cpp.
References MPU9250::_interface, _parent, and device::Device::read().
Referenced by passthrough_read().
|
protected |
Definition at line 181 of file MPU9250_mag.cpp.
References _interface, passthrough_read(), and device::Device::read().
Referenced by ak8963_check_id().
void MPU9250_mag::set_passthrough | ( | uint8_t | reg, |
uint8_t | size, | ||
uint8_t * | out = NULL |
||
) |
Definition at line 146 of file MPU9250_mag.cpp.
References _parent, AK8963_I2C_ADDR, BIT_I2C_READ_FLAG, BIT_I2C_SLV0_EN, MPUREG_I2C_SLV0_ADDR, MPUREG_I2C_SLV0_CTRL, MPUREG_I2C_SLV0_D0, MPUREG_I2C_SLV0_REG, and MPU9250::write_reg().
Referenced by ak8963_setup(), passthrough_read(), and passthrough_write().
|
protected |
Definition at line 215 of file MPU9250_mag.cpp.
References _interface, MPU9250_LOW_SPEED_OP, passthrough_write(), and device::Device::write().
Referenced by ak8963_read_adjustments(), ak8963_reset(), and ak8963_setup().
|
friend |
Definition at line 134 of file MPU9250_mag.h.
|
protected |
Definition at line 132 of file MPU9250_mag.h.
Referenced by ak8963_read_adjustments(), ak8963_setup(), ak8963_setup_master_i2c(), MPU9250::init(), measure(), read_reg(), and write_reg().
|
private |
Definition at line 153 of file MPU9250_mag.h.
Referenced by measure(), and ~MPU9250_mag().
|
private |
Definition at line 152 of file MPU9250_mag.h.
Referenced by measure(), and ~MPU9250_mag().
|
private |
Definition at line 151 of file MPU9250_mag.h.
Referenced by measure(), and ~MPU9250_mag().
|
private |
Definition at line 149 of file MPU9250_mag.h.
|
private |
Definition at line 147 of file MPU9250_mag.h.
Referenced by _measure(), ak8963_setup(), ak8963_setup_master_i2c(), passthrough_read(), passthrough_write(), read_block(), and set_passthrough().
|
private |
Definition at line 145 of file MPU9250_mag.h.
Referenced by _measure(), ak8963_read_adjustments(), measure(), and MPU9250_mag().