PX4 Firmware
PX4 Autopilot Software http://px4.io
QMC5883 Class Reference
Inheritance diagram for QMC5883:
Collaboration diagram for QMC5883:

Classes

struct  _last_report
 used for info() More...
 

Public Member Functions

 QMC5883 (device::Device *interface, const char *path, enum Rotation rotation)
 
virtual ~QMC5883 ()
 
virtual int init ()
 
virtual ssize_t read (struct file *filp, char *buffer, size_t buflen)
 
virtual int ioctl (struct file *filp, int cmd, unsigned long arg)
 
void stop ()
 Stop the automatic measurement state machine. More...
 
void print_info ()
 Diagnostics - print some basic information about the driver. More...
 
- Public Member Functions inherited from device::CDev
 CDev (const char *name, const char *devname)
 Constructor. More...
 
virtual ~CDev ()=default
 
virtual int ioctl (file_t *filep, int cmd, unsigned long arg)
 Perform an ioctl operation on the device. More...
 
- Public Member Functions inherited from device::Device
 Device (const Device &)=delete
 
Deviceoperator= (const Device &)=delete
 
 Device (Device &&)=delete
 
Deviceoperator= (Device &&)=delete
 
virtual ~Device ()=default
 Destructor. More...
 
virtual int read (unsigned address, void *data, unsigned count)
 Read directly from the device. More...
 
virtual int write (unsigned address, void *data, unsigned count)
 Write directly to the device. More...
 
virtual int ioctl (unsigned operation, unsigned &arg)
 Perform a device-specific operation. More...
 
uint32_t get_device_id () const
 
DeviceBusType get_device_bus_type () const
 Return the bus type the device is connected to. More...
 
void set_device_bus_type (DeviceBusType bus_type)
 
uint8_t get_device_bus () const
 Return the bus ID the device is connected to. More...
 
void set_device_bus (uint8_t bus)
 
uint8_t get_device_address () const
 Return the bus address of the device. More...
 
void set_device_address (int address)
 
uint8_t get_device_type () const
 Return the device type. More...
 
void set_device_type (uint8_t devtype)
 
virtual bool external () const
 
- Public Member Functions inherited from cdev::CDev
 CDev (const char *devname)
 Constructor. More...
 
 CDev (const CDev &)=delete
 
CDevoperator= (const CDev &)=delete
 
 CDev (CDev &&)=delete
 
CDevoperator= (CDev &&)=delete
 
virtual int open (file_t *filep)
 Handle an open of the device. More...
 
virtual int close (file_t *filep)
 Handle a close of the device. More...
 
virtual ssize_t read (file_t *filep, char *buffer, size_t buflen)
 Perform a read from the device. More...
 
virtual ssize_t write (file_t *filep, const char *buffer, size_t buflen)
 Perform a write to the device. More...
 
virtual off_t seek (file_t *filep, off_t offset, int whence)
 Perform a logical seek operation on the device. More...
 
virtual int poll (file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 Perform a poll setup/teardown operation. More...
 
const char * get_devname () const
 Get the device name. More...
 

Protected Attributes

Device_interface
 
- Protected Attributes inherited from device::CDev
bool _pub_blocked {false}
 true if publishing should be blocked More...
 
- Protected Attributes inherited from device::Device
const char * _name {nullptr}
 driver name More...
 
bool _debug_enabled {false}
 if true, debug messages are printed More...
 
- Protected Attributes inherited from cdev::CDev
px4_sem_t _lock
 lock to protect access to all class members (also for derived classes) More...
 

Private Member Functions

void start ()
 Initialise the automatic measurement state machine and start it. More...
 
int reset ()
 Reset the device. More...
 
void check_conf (void)
 check the sensor configuration. More...
 
void Run () override
 Perform a poll cycle; collect from the previous measurement and start a new one. More...
 
int write_reg (uint8_t reg, uint8_t val)
 Write a register. More...
 
int read_reg (uint8_t reg, uint8_t &val)
 Read a register. More...
 
int collect ()
 Collect the result of the most recent measurement. More...
 
 QMC5883 (const QMC5883 &)
 
QMC5883 operator= (const QMC5883 &)
 

Private Attributes

unsigned _measure_interval {0}
 
ringbuffer::RingBuffer * _reports
 
struct mag_calibration_s _scale
 
float _range_scale
 
float _range_ga
 
bool _collect_phase
 
int _class_instance
 
int _orb_class_instance
 
orb_advert_t _mag_topic
 
perf_counter_t _sample_perf
 
perf_counter_t _comms_errors
 
perf_counter_t _range_errors
 
perf_counter_t _conf_errors
 
bool _sensor_ok
 sensor was found and reports ok More...
 
enum Rotation _rotation
 
uint8_t _range_bits
 
uint8_t _conf_reg
 
uint8_t _temperature_counter
 
uint8_t _temperature_error_count
 

Additional Inherited Members

- Public Types inherited from device::Device
enum  DeviceBusType {
  DeviceBusType_UNKNOWN = 0, DeviceBusType_I2C = 1, DeviceBusType_SPI = 2, DeviceBusType_UAVCAN = 3,
  DeviceBusType_SIMULATION = 4
}
 Device bus types for DEVID. More...
 
- Static Public Member Functions inherited from device::Device
static const char * get_device_bus_string (DeviceBusType bus)
 
static int device_id_print_buffer (char *buffer, int length, uint32_t id)
 Print decoded device id string to a buffer. More...
 
- Protected Member Functions inherited from device::Device
 Device (const char *name)
 
 Device (const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype=0)
 
 Device (DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype=0)
 
- Protected Member Functions inherited from cdev::CDev
virtual pollevent_t poll_state (file_t *filep)
 Check the current state of the device for poll events from the perspective of the file. More...
 
virtual void poll_notify (pollevent_t events)
 Report new poll events. More...
 
virtual void poll_notify_one (px4_pollfd_struct_t *fds, pollevent_t events)
 Internal implementation of poll_notify. More...
 
virtual int open_first (file_t *filep)
 Notification of the first open. More...
 
virtual int close_last (file_t *filep)
 Notification of the last close. More...
 
virtual int register_class_devname (const char *class_devname)
 Register a class device name, automatically adding device class instance suffix if need be. More...
 
virtual int unregister_class_devname (const char *class_devname, unsigned class_instance)
 Register a class device name, automatically adding device class instance suffix if need be. More...
 
void lock ()
 Take the driver lock. More...
 
void unlock ()
 Release the driver lock. More...
 
int unregister_driver_and_memory ()
 First, unregisters the driver. More...
 
- Static Protected Attributes inherited from cdev::CDev
static const px4_file_operations_t fops = {}
 Pointer to the default cdev file operations table; useful for registering clone devices etc. More...
 

Detailed Description

Definition at line 137 of file qmc5883.cpp.

Constructor & Destructor Documentation

◆ QMC5883() [1/2]

QMC5883::QMC5883 ( device::Device interface,
const char *  path,
enum Rotation  rotation 
)

Definition at line 262 of file qmc5883.cpp.

References _class_instance, _collect_phase, _comms_errors, _conf_errors, _conf_reg, _interface, _mag_topic, _orb_class_instance, _range_bits, _range_errors, _range_ga, _range_scale, _rotation, _sample_perf, _scale, _sensor_ok, _temperature_counter, _temperature_error_count, DRV_MAG_DEVTYPE_QMC5883, f(), PC_COUNT, PC_ELAPSED, perf_alloc, mag_calibration_s::x_offset, mag_calibration_s::x_scale, mag_calibration_s::y_offset, mag_calibration_s::y_scale, mag_calibration_s::z_offset, and mag_calibration_s::z_scale.

Referenced by qmc5883::start_bus().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ~QMC5883()

QMC5883::~QMC5883 ( )
virtual

Definition at line 300 of file qmc5883.cpp.

References _class_instance, _comms_errors, _conf_errors, _range_errors, _reports, _sample_perf, MAG_BASE_DEVICE_PATH, perf_free(), stop(), and cdev::CDev::unregister_class_devname().

Here is the call graph for this function:

◆ QMC5883() [2/2]

QMC5883::QMC5883 ( const QMC5883 )
private

Member Function Documentation

◆ check_conf()

void QMC5883::check_conf ( void  )
private

check the sensor configuration.

check that the configuration register has the right value.

checks that the config of the sensor is correctly set, to cope with communication errors causing the configuration to change

This is done periodically to cope with I2C bus noise causing the configuration of the compass to change.

Definition at line 356 of file qmc5883.cpp.

References _comms_errors, _conf_errors, _conf_reg, OK, perf_count(), QMC5883_ADDR_CONTROL_1, read_reg(), and write_reg().

Referenced by collect().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ collect()

int QMC5883::collect ( )
private

Collect the result of the most recent measurement.

Definition at line 603 of file qmc5883.cpp.

References _comms_errors, _interface, _mag_topic, _orb_class_instance, device::CDev::_pub_blocked, _range_scale, _reports, _rotation, _sample_perf, _scale, _temperature_counter, matrix::abs(), check_conf(), DEVICE_DEBUG, hrt_absolute_time(), mag_report, MAGIOCGEXTERNAL, OK, orb_advertise_multi(), ORB_ID, ORB_PRIO_HIGH, ORB_PRIO_MAX, orb_publish(), perf_begin(), perf_count(), perf_end(), perf_event_count(), cdev::CDev::poll_notify(), QMC5883_ADDR_DATA_OUT_X_LSB, QMC5883_ADDR_TEMP_OUT_LSB, QMC5883_MAX_COUNT, QMC5883_TEMP_OFFSET, rotate_3f(), mag_calibration_s::x_offset, mag_calibration_s::x_scale, mag_calibration_s::y_offset, mag_calibration_s::y_scale, mag_calibration_s::z_offset, and mag_calibration_s::z_scale.

Referenced by read(), and Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ init()

int QMC5883::init ( )
virtual

Reimplemented from device::CDev.

Definition at line 321 of file qmc5883.cpp.

References _class_instance, _reports, _sensor_ok, DEVICE_DEBUG, ToneAlarmInterface::init(), MAG_BASE_DEVICE_PATH, mag_report, OK, cdev::CDev::register_class_devname(), and reset().

Here is the call graph for this function:

◆ ioctl()

int QMC5883::ioctl ( struct file filp,
int  cmd,
unsigned long  arg 
)
virtual

Definition at line 431 of file qmc5883.cpp.

References _interface, _measure_interval, _scale, DEVICE_DEBUG, MAGIOCGEXTERNAL, MAGIOCGSCALE, MAGIOCSRANGE, MAGIOCSSCALE, OK, QMC5883_CONVERSION_INTERVAL, reset(), SENSOR_POLLRATE_DEFAULT, SENSORIOCRESET, SENSORIOCSPOLLRATE, and start().

Referenced by qmc5883::reset(), qmc5883::start_bus(), and qmc5883::test().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ operator=()

QMC5883 QMC5883::operator= ( const QMC5883 )
private

◆ print_info()

void QMC5883::print_info ( )

Diagnostics - print some basic information about the driver.

Definition at line 791 of file qmc5883.cpp.

References _comms_errors, _measure_interval, _reports, _sample_perf, and perf_print_counter().

Referenced by qmc5883::info().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ read()

ssize_t QMC5883::read ( struct file filp,
char *  buffer,
size_t  buflen 
)
virtual

Definition at line 379 of file qmc5883.cpp.

References _measure_interval, _reports, collect(), mag_report, OK, and QMC5883_CONVERSION_INTERVAL.

Referenced by qmc5883::test().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ read_reg()

int QMC5883::read_reg ( uint8_t  reg,
uint8_t &  val 
)
private

Read a register.

Parameters
regThe register to read.
valThe value read.
Returns
OK on read success.

Definition at line 782 of file qmc5883.cpp.

References _interface.

Referenced by check_conf(), and reset().

Here is the caller graph for this function:

◆ reset()

int QMC5883::reset ( )
private

Reset the device.

Definition at line 533 of file qmc5883.cpp.

References _conf_reg, _range_bits, _range_ga, _range_scale, OK, QMC5883_ADDR_CONTROL_1, QMC5883_ADDR_CONTROL_2, QMC5883_ADDR_DATA_OUT_X_LSB, QMC5883_ADDR_SET_RESET, QMC5883_MODE_REG_CONTINOUS_MODE, QMC5883_OUTPUT_DATA_RATE_200, QMC5883_OUTPUT_RANGE_2G, QMC5883_OVERSAMPLE_512, QMC5883_SET_DEFAULT, QMC5883_SOFT_RESET, read_reg(), and write_reg().

Referenced by init(), and ioctl().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ Run()

void QMC5883::Run ( )
overrideprivate

Perform a poll cycle; collect from the previous measurement and start a new one.

This is the heart of the measurement state machine. This function alternately starts a measurement, or collects the data from the previous measurement.

When the interval between measurements is greater than the minimum measurement interval, a gap is inserted between collection and measurement to provide the most recent measurement possible at the next interval.

Definition at line 561 of file qmc5883.cpp.

References _collect_phase, _measure_interval, collect(), DEVICE_DEBUG, OK, QMC5883_CONVERSION_INTERVAL, and start().

Here is the call graph for this function:

◆ start()

void QMC5883::start ( )
private

Initialise the automatic measurement state machine and start it.

Note
This function is called at open and error time. It might make sense to make it more aggressive about resetting the bus in case of errors.

Definition at line 512 of file qmc5883.cpp.

References _collect_phase, and _reports.

Referenced by ioctl(), and Run().

Here is the caller graph for this function:

◆ stop()

void QMC5883::stop ( )

Stop the automatic measurement state machine.

Definition at line 523 of file qmc5883.cpp.

References _measure_interval.

Referenced by qmc5883::stop(), and ~QMC5883().

Here is the caller graph for this function:

◆ write_reg()

int QMC5883::write_reg ( uint8_t  reg,
uint8_t  val 
)
private

Write a register.

Parameters
regThe register to write.
valThe value to write.
Returns
OK on write success.

Definition at line 775 of file qmc5883.cpp.

References _interface.

Referenced by check_conf(), and reset().

Here is the caller graph for this function:

Member Data Documentation

◆ _class_instance

int QMC5883::_class_instance
private

Definition at line 169 of file qmc5883.cpp.

Referenced by init(), QMC5883(), and ~QMC5883().

◆ _collect_phase

bool QMC5883::_collect_phase
private

Definition at line 168 of file qmc5883.cpp.

Referenced by QMC5883(), Run(), and start().

◆ _comms_errors

perf_counter_t QMC5883::_comms_errors
private

Definition at line 175 of file qmc5883.cpp.

Referenced by check_conf(), collect(), print_info(), QMC5883(), and ~QMC5883().

◆ _conf_errors

perf_counter_t QMC5883::_conf_errors
private

Definition at line 177 of file qmc5883.cpp.

Referenced by check_conf(), QMC5883(), and ~QMC5883().

◆ _conf_reg

uint8_t QMC5883::_conf_reg
private

Definition at line 187 of file qmc5883.cpp.

Referenced by check_conf(), QMC5883(), and reset().

◆ _interface

Device* QMC5883::_interface
protected

Definition at line 159 of file qmc5883.cpp.

Referenced by collect(), ioctl(), QMC5883(), read_reg(), and write_reg().

◆ _mag_topic

orb_advert_t QMC5883::_mag_topic
private

Definition at line 172 of file qmc5883.cpp.

Referenced by collect(), and QMC5883().

◆ _measure_interval

unsigned QMC5883::_measure_interval {0}
private

Definition at line 162 of file qmc5883.cpp.

Referenced by ioctl(), print_info(), read(), Run(), and stop().

◆ _orb_class_instance

int QMC5883::_orb_class_instance
private

Definition at line 170 of file qmc5883.cpp.

Referenced by collect(), and QMC5883().

◆ _range_bits

uint8_t QMC5883::_range_bits
private

Definition at line 186 of file qmc5883.cpp.

Referenced by QMC5883(), and reset().

◆ _range_errors

perf_counter_t QMC5883::_range_errors
private

Definition at line 176 of file qmc5883.cpp.

Referenced by QMC5883(), and ~QMC5883().

◆ _range_ga

float QMC5883::_range_ga
private

Definition at line 167 of file qmc5883.cpp.

Referenced by QMC5883(), and reset().

◆ _range_scale

float QMC5883::_range_scale
private

Definition at line 166 of file qmc5883.cpp.

Referenced by collect(), QMC5883(), and reset().

◆ _reports

ringbuffer::RingBuffer* QMC5883::_reports
private

Definition at line 164 of file qmc5883.cpp.

Referenced by collect(), init(), print_info(), read(), start(), and ~QMC5883().

◆ _rotation

enum Rotation QMC5883::_rotation
private

Definition at line 182 of file qmc5883.cpp.

Referenced by collect(), and QMC5883().

◆ _sample_perf

perf_counter_t QMC5883::_sample_perf
private

Definition at line 174 of file qmc5883.cpp.

Referenced by collect(), print_info(), QMC5883(), and ~QMC5883().

◆ _scale

struct mag_calibration_s QMC5883::_scale
private

Definition at line 165 of file qmc5883.cpp.

Referenced by collect(), ioctl(), and QMC5883().

◆ _sensor_ok

bool QMC5883::_sensor_ok
private

sensor was found and reports ok

Definition at line 180 of file qmc5883.cpp.

Referenced by init(), and QMC5883().

◆ _temperature_counter

uint8_t QMC5883::_temperature_counter
private

Definition at line 188 of file qmc5883.cpp.

Referenced by collect(), and QMC5883().

◆ _temperature_error_count

uint8_t QMC5883::_temperature_error_count
private

Definition at line 189 of file qmc5883.cpp.

Referenced by QMC5883().


The documentation for this class was generated from the following file: