PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <PX4Magnetometer.hpp>
Public Member Functions | |
PX4Magnetometer (uint32_t device_id, uint8_t priority, enum Rotation rotation) | |
~PX4Magnetometer () override | |
int | ioctl (cdev::file_t *filp, int cmd, unsigned long arg) override |
Perform an ioctl operation on the device. More... | |
void | set_device_type (uint8_t devtype) |
void | set_error_count (uint64_t error_count) |
void | set_scale (float scale) |
void | set_temperature (float temperature) |
void | set_external (bool external) |
void | set_sensitivity (float x, float y, float z) |
void | update (hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) |
void | print_status () |
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 | ~CDev () |
virtual int | init () |
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... | |
Private Attributes | |
uORB::PublicationMultiData< sensor_mag_s > | _sensor_mag_pub |
enum Rotation | _rotation |
matrix::Vector3f | _calibration_scale {1.0f, 1.0f, 1.0f} |
matrix::Vector3f | _calibration_offset {0.0f, 0.0f, 0.0f} |
matrix::Vector3f | _sensitivity {1.0f, 1.0f, 1.0f} |
int | _class_device_instance {-1} |
Additional Inherited Members | |
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... | |
Protected Attributes inherited from cdev::CDev | |
px4_sem_t | _lock |
lock to protect access to all class members (also for derived classes) 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 44 of file PX4Magnetometer.hpp.
PX4Magnetometer::PX4Magnetometer | ( | uint32_t | device_id, |
uint8_t | priority, | ||
enum Rotation | rotation | ||
) |
Definition at line 39 of file PX4Magnetometer.cpp.
References _class_device_instance, _rotation, _sensor_mag_pub, sensor_mag_s::device_id, device_id, uORB::PublicationMultiData< T >::get(), MAG_BASE_DEVICE_PATH, ORB_ID, cdev::CDev::register_class_devname(), and sensor_mag_s::scaling.
|
override |
Definition at line 50 of file PX4Magnetometer.cpp.
References _class_device_instance, MAG_BASE_DEVICE_PATH, and cdev::CDev::unregister_class_devname().
|
overridevirtual |
Perform an ioctl operation on the device.
The default implementation handles DIOC_GETPRIV, and otherwise returns -ENOTTY. Subclasses should call the default implementation for any command they do not handle themselves.
filep | Pointer to the NuttX file structure. |
cmd | The ioctl command value. |
arg | The ioctl argument value. |
Reimplemented from cdev::CDev.
Definition at line 58 of file PX4Magnetometer.cpp.
References _calibration_offset, _calibration_scale, _sensor_mag_pub, sensor_mag_s::device_id, uORB::PublicationMultiData< T >::get(), MAGIOCGSCALE, MAGIOCSSCALE, and mag_calibration_s::x_offset.
void PX4Magnetometer::print_status | ( | ) |
Definition at line 139 of file PX4Magnetometer.cpp.
References _calibration_offset, _calibration_scale, _class_device_instance, _sensor_mag_pub, uORB::PublicationMultiData< T >::get(), and MAG_BASE_DEVICE_PATH.
Referenced by AK09916::print_info(), ADIS16448::print_info(), and set_sensitivity().
void PX4Magnetometer::set_device_type | ( | uint8_t | devtype | ) |
Definition at line 95 of file PX4Magnetometer.cpp.
References _sensor_mag_pub, sensor_mag_s::device_id, device::Device::DeviceId::devid, device::Device::DeviceId::devid_s, device::Device::DeviceStructure::devtype, and uORB::PublicationMultiData< T >::get().
Referenced by ADIS16448::ADIS16448(), AK09916::AK09916(), FXOS8701CQ::FXOS8701CQ(), ICM20948_mag::ICM20948_mag(), LSM303D::LSM303D(), and MPU9250_mag::MPU9250_mag().
|
inline |
Definition at line 54 of file PX4Magnetometer.hpp.
References _sensor_mag_pub, sensor_mag_s::error_count, and uORB::PublicationMultiData< T >::get().
Referenced by AK09916::measure(), ICM20948_mag::measure(), MPU9250_mag::measure(), ADIS16448::measure(), and LSM303D::measureMagnetometer().
|
inline |
Definition at line 57 of file PX4Magnetometer.hpp.
References _sensor_mag_pub, uORB::PublicationMultiData< T >::get(), and sensor_mag_s::is_external.
Referenced by ICM20948_mag::_measure(), MPU9250_mag::_measure(), ADIS16448::ADIS16448(), LSM303D::LSM303D(), AK09916::measure(), and LSM303D::measureMagnetometer().
|
inline |
Definition at line 55 of file PX4Magnetometer.hpp.
References _sensor_mag_pub, uORB::PublicationMultiData< T >::get(), and sensor_mag_s::scaling.
Referenced by ADIS16448::ADIS16448(), AK09916::AK09916(), FXOS8701CQ::FXOS8701CQ(), ICM20948_mag::ICM20948_mag(), FXOS8701CQ::mag_set_range(), LSM303D::mag_set_range(), MPU9250_mag::MPU9250_mag(), and Sih::send_IMU().
|
inline |
Definition at line 58 of file PX4Magnetometer.hpp.
References _sensitivity, hrt_abstime, print_status(), and update().
Referenced by ICM20948_mag::ak09916_read_adjustments(), and MPU9250_mag::ak8963_read_adjustments().
|
inline |
Definition at line 56 of file PX4Magnetometer.hpp.
References _sensor_mag_pub, uORB::PublicationMultiData< T >::get(), and sensor_mag_s::temperature.
Referenced by ICM20948_mag::_measure(), MPU9250_mag::_measure(), ADIS16448::measure(), LSM303D::measureMagnetometer(), and Sih::send_IMU().
void PX4Magnetometer::update | ( | hrt_abstime | timestamp, |
int16_t | x, | ||
int16_t | y, | ||
int16_t | z | ||
) |
Definition at line 109 of file PX4Magnetometer.cpp.
References _calibration_offset, _calibration_scale, _rotation, _sensitivity, _sensor_mag_pub, uORB::PublicationMultiData< T >::get(), cdev::CDev::poll_notify(), rotate_3f(), sensor_mag_s::scaling, sensor_mag_s::timestamp, uORB::PublicationMultiData< T >::update(), sensor_mag_s::x, sensor_mag_s::x_raw, sensor_mag_s::y, sensor_mag_s::y_raw, sensor_mag_s::z, and sensor_mag_s::z_raw.
Referenced by ICM20948_mag::_measure(), MPU9250_mag::_measure(), AK09916::measure(), ADIS16448::measure(), LSM303D::measureMagnetometer(), FXOS8701CQ::Run(), Sih::send_IMU(), and set_sensitivity().
|
private |
Definition at line 71 of file PX4Magnetometer.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 70 of file PX4Magnetometer.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 75 of file PX4Magnetometer.hpp.
Referenced by print_status(), PX4Magnetometer(), and ~PX4Magnetometer().
|
private |
Definition at line 68 of file PX4Magnetometer.hpp.
Referenced by PX4Magnetometer(), and update().
|
private |
Definition at line 73 of file PX4Magnetometer.hpp.
Referenced by set_sensitivity(), and update().
|
private |
Definition at line 66 of file PX4Magnetometer.hpp.
Referenced by ioctl(), print_status(), PX4Magnetometer(), set_device_type(), set_error_count(), set_external(), set_scale(), set_temperature(), and update().