PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <PX4Accelerometer.hpp>
Public Member Functions | |
PX4Accelerometer (uint32_t device_id, uint8_t priority=ORB_PRIO_DEFAULT, enum Rotation rotation=ROTATION_NONE) | |
~PX4Accelerometer () 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_sample_rate (unsigned rate) |
void | update (hrt_abstime timestamp, float x, float y, float 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 Member Functions | |
void | configure_filter (float cutoff_freq) |
Private Attributes | |
uORB::PublicationMultiData< sensor_accel_s > | _sensor_accel_pub |
math::LowPassFilter2pVector3f | _filter {1000, 100} |
Integrator | _integrator {4000, false} |
enum Rotation | _rotation |
matrix::Vector3f | _calibration_scale {1.0f, 1.0f, 1.0f} |
matrix::Vector3f | _calibration_offset {0.0f, 0.0f, 0.0f} |
int | _class_device_instance {-1} |
unsigned | _sample_rate {1000} |
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 47 of file PX4Accelerometer.hpp.
PX4Accelerometer::PX4Accelerometer | ( | uint32_t | device_id, |
uint8_t | priority = ORB_PRIO_DEFAULT , |
||
enum Rotation | rotation = ROTATION_NONE |
||
) |
Definition at line 39 of file PX4Accelerometer.cpp.
References _class_device_instance, _rotation, _sensor_accel_pub, ACCEL_BASE_DEVICE_PATH, configure_filter(), sensor_accel_s::device_id, device_id, uORB::PublicationMultiData< T >::get(), ORB_ID, cdev::CDev::register_class_devname(), and sensor_accel_s::scaling.
|
override |
Definition at line 55 of file PX4Accelerometer.cpp.
References _class_device_instance, ACCEL_BASE_DEVICE_PATH, and cdev::CDev::unregister_class_devname().
|
inlineprivate |
Definition at line 69 of file PX4Accelerometer.hpp.
References _filter, _sample_rate, and math::LowPassFilter2pVector3f::set_cutoff_frequency().
Referenced by PX4Accelerometer().
|
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 63 of file PX4Accelerometer.cpp.
References _calibration_offset, _calibration_scale, _sensor_accel_pub, ACCELIOCSSCALE, sensor_accel_s::device_id, and uORB::PublicationMultiData< T >::get().
void PX4Accelerometer::print_status | ( | ) |
Definition at line 149 of file PX4Accelerometer.cpp.
References _calibration_offset, _calibration_scale, _class_device_instance, _filter, _sample_rate, _sensor_accel_pub, ACCEL_BASE_DEVICE_PATH, uORB::PublicationMultiData< T >::get(), and math::LowPassFilter2pVector3f::get_cutoff_freq().
Referenced by ADIS16477::print_info(), ADIS16497::print_info(), ADIS16448::print_info(), BMI055_accel::print_info(), BMI088_accel::print_info(), MPU9250::print_info(), BMI160::print_info(), MPU6000::print_info(), ICM20948::print_info(), and set_temperature().
void PX4Accelerometer::set_device_type | ( | uint8_t | devtype | ) |
Definition at line 86 of file PX4Accelerometer.cpp.
References _sensor_accel_pub, sensor_accel_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(), ADIS16477::ADIS16477(), ADIS16497::ADIS16497(), BMI055_accel::BMI055_accel(), BMI088_accel::BMI088_accel(), BMI160::BMI160(), FXOS8701CQ::FXOS8701CQ(), ICM20948::ICM20948(), LSM303D::LSM303D(), MPU6000::MPU6000(), and MPU9250::MPU9250().
|
inline |
Definition at line 57 of file PX4Accelerometer.hpp.
References _sensor_accel_pub, sensor_accel_s::error_count, and uORB::PublicationMultiData< T >::get().
Referenced by ADIS16448::measure(), ADIS16497::measure(), BMI055_accel::measure(), BMI088_accel::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), ICM20948::measure(), LSM303D::measureAccelerometer(), and FXOS8701CQ::Run().
void PX4Accelerometer::set_sample_rate | ( | unsigned | rate | ) |
Definition at line 100 of file PX4Accelerometer.cpp.
References _filter, _sample_rate, math::LowPassFilter2pVector3f::get_cutoff_freq(), and math::LowPassFilter2pVector3f::set_cutoff_frequency().
Referenced by LSM303D::accel_set_samplerate(), ADIS16477::ADIS16477(), ADIS16497::ADIS16497(), BMI160::BMI160(), ADIS16448::set_sample_rate(), and set_temperature().
|
inline |
Definition at line 58 of file PX4Accelerometer.hpp.
References _sensor_accel_pub, uORB::PublicationMultiData< T >::get(), and sensor_accel_s::scaling.
Referenced by FXOS8701CQ::accel_set_range(), LSM303D::accel_set_range(), ADIS16448::ADIS16448(), ADIS16477::ADIS16477(), ADIS16497::ADIS16497(), Sih::send_IMU(), BMI055_accel::set_accel_range(), BMI088_accel::set_accel_range(), BMI160::set_accel_range(), MPU9250::set_accel_range(), MPU6000::set_accel_range(), and ICM20948::set_accel_range().
|
inline |
Definition at line 59 of file PX4Accelerometer.hpp.
References _sensor_accel_pub, uORB::PublicationMultiData< T >::get(), hrt_abstime, print_status(), set_sample_rate(), sensor_accel_s::temperature, and update().
Referenced by ADIS16477::measure(), ADIS16448::measure(), ADIS16497::measure(), BMI055_accel::measure(), BMI088_accel::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), ICM20948::measure(), LSM303D::measureMagnetometer(), FXOS8701CQ::Run(), and Sih::send_IMU().
void PX4Accelerometer::update | ( | hrt_abstime | timestamp, |
float | x, | ||
float | y, | ||
float | z | ||
) |
Definition at line 107 of file PX4Accelerometer.cpp.
References _calibration_offset, _calibration_scale, _filter, _integrator, _rotation, _sensor_accel_pub, math::LowPassFilter2pVector3f::apply(), uORB::PublicationMultiData< T >::get(), sensor_accel_s::integral_dt, cdev::CDev::poll_notify(), Integrator::put(), rotate_3f(), sensor_accel_s::scaling, sensor_accel_s::timestamp, uORB::PublicationMultiData< T >::update(), sensor_accel_s::x, sensor_accel_s::x_integral, sensor_accel_s::x_raw, sensor_accel_s::y, sensor_accel_s::y_integral, sensor_accel_s::y_raw, sensor_accel_s::z, sensor_accel_s::z_integral, and sensor_accel_s::z_raw.
Referenced by ADIS16477::measure(), ADIS16448::measure(), ADIS16497::measure(), BMI055_accel::measure(), BMI088_accel::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), ICM20948::measure(), LSM303D::measureAccelerometer(), FXOS8701CQ::Run(), Sih::send_IMU(), and set_temperature().
|
private |
Definition at line 79 of file PX4Accelerometer.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 78 of file PX4Accelerometer.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 81 of file PX4Accelerometer.hpp.
Referenced by print_status(), PX4Accelerometer(), and ~PX4Accelerometer().
|
private |
Definition at line 73 of file PX4Accelerometer.hpp.
Referenced by configure_filter(), print_status(), set_sample_rate(), and update().
|
private |
Definition at line 74 of file PX4Accelerometer.hpp.
Referenced by update().
|
private |
Definition at line 76 of file PX4Accelerometer.hpp.
Referenced by PX4Accelerometer(), and update().
|
private |
Definition at line 83 of file PX4Accelerometer.hpp.
Referenced by configure_filter(), print_status(), and set_sample_rate().
|
private |
Definition at line 71 of file PX4Accelerometer.hpp.
Referenced by ioctl(), print_status(), PX4Accelerometer(), set_device_type(), set_error_count(), set_scale(), set_temperature(), and update().