PX4 Firmware
PX4 Autopilot Software http://px4.io
|
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 | |
Device & | operator= (const Device &)=delete |
Device (Device &&)=delete | |
Device & | operator= (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 | |
CDev & | operator= (const CDev &)=delete |
CDev (CDev &&)=delete | |
CDev & | operator= (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... | |
Definition at line 137 of file qmc5883.cpp.
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().
|
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().
|
private |
|
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().
|
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().
|
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().
|
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().
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().
|
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().
|
private |
Read a register.
reg | The register to read. |
val | The value read. |
Definition at line 782 of file qmc5883.cpp.
References _interface.
Referenced by check_conf(), and 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().
|
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().
|
private |
Initialise the automatic measurement state machine and start it.
Definition at line 512 of file qmc5883.cpp.
References _collect_phase, and _reports.
Referenced by ioctl(), and Run().
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().
|
private |
Write a register.
reg | The register to write. |
val | The value to write. |
Definition at line 775 of file qmc5883.cpp.
References _interface.
Referenced by check_conf(), and reset().
|
private |
Definition at line 169 of file qmc5883.cpp.
Referenced by init(), QMC5883(), and ~QMC5883().
|
private |
Definition at line 168 of file qmc5883.cpp.
|
private |
Definition at line 175 of file qmc5883.cpp.
Referenced by check_conf(), collect(), print_info(), QMC5883(), and ~QMC5883().
|
private |
Definition at line 177 of file qmc5883.cpp.
Referenced by check_conf(), QMC5883(), and ~QMC5883().
|
private |
Definition at line 187 of file qmc5883.cpp.
Referenced by check_conf(), QMC5883(), and reset().
|
protected |
Definition at line 159 of file qmc5883.cpp.
Referenced by collect(), ioctl(), QMC5883(), read_reg(), and write_reg().
|
private |
Definition at line 172 of file qmc5883.cpp.
|
private |
Definition at line 162 of file qmc5883.cpp.
Referenced by ioctl(), print_info(), read(), Run(), and stop().
|
private |
Definition at line 170 of file qmc5883.cpp.
|
private |
Definition at line 186 of file qmc5883.cpp.
|
private |
Definition at line 176 of file qmc5883.cpp.
Referenced by QMC5883(), and ~QMC5883().
|
private |
Definition at line 167 of file qmc5883.cpp.
|
private |
Definition at line 166 of file qmc5883.cpp.
|
private |
Definition at line 164 of file qmc5883.cpp.
Referenced by collect(), init(), print_info(), read(), start(), and ~QMC5883().
|
private |
Definition at line 182 of file qmc5883.cpp.
|
private |
Definition at line 174 of file qmc5883.cpp.
Referenced by collect(), print_info(), QMC5883(), and ~QMC5883().
|
private |
Definition at line 165 of file qmc5883.cpp.
|
private |
sensor was found and reports ok
Definition at line 180 of file qmc5883.cpp.
|
private |
Definition at line 188 of file qmc5883.cpp.
|
private |
Definition at line 189 of file qmc5883.cpp.
Referenced by QMC5883().