PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <PX4Gyroscope.hpp>
Public Member Functions | |
PX4Gyroscope (uint32_t device_id, uint8_t priority=ORB_PRIO_DEFAULT, enum Rotation rotation=ROTATION_NONE) | |
~PX4Gyroscope () 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_gyro_s > | _sensor_gyro_pub |
uORB::PublicationMultiData< sensor_gyro_control_s > | _sensor_gyro_control_pub |
math::LowPassFilter2pVector3f | _filter {1000, 100} |
Integrator | _integrator {4000, true} |
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 48 of file PX4Gyroscope.hpp.
PX4Gyroscope::PX4Gyroscope | ( | uint32_t | device_id, |
uint8_t | priority = ORB_PRIO_DEFAULT , |
||
enum Rotation | rotation = ROTATION_NONE |
||
) |
Definition at line 39 of file PX4Gyroscope.cpp.
References _class_device_instance, _rotation, _sensor_gyro_control_pub, _sensor_gyro_pub, configure_filter(), sensor_gyro_control_s::device_id, sensor_gyro_s::device_id, device_id, uORB::PublicationMultiData< T >::get(), GYRO_BASE_DEVICE_PATH, ORB_ID, cdev::CDev::register_class_devname(), and sensor_gyro_s::scaling.
|
override |
Definition at line 57 of file PX4Gyroscope.cpp.
References _class_device_instance, GYRO_BASE_DEVICE_PATH, and cdev::CDev::unregister_class_devname().
|
inlineprivate |
Definition at line 70 of file PX4Gyroscope.hpp.
References _filter, _sample_rate, and math::LowPassFilter2pVector3f::set_cutoff_frequency().
Referenced by PX4Gyroscope().
|
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 65 of file PX4Gyroscope.cpp.
References _calibration_offset, _calibration_scale, _sensor_gyro_pub, sensor_gyro_s::device_id, uORB::PublicationMultiData< T >::get(), and GYROIOCSSCALE.
void PX4Gyroscope::print_status | ( | ) |
Definition at line 173 of file PX4Gyroscope.cpp.
References _calibration_offset, _calibration_scale, _class_device_instance, _filter, _sample_rate, _sensor_gyro_control_pub, _sensor_gyro_pub, uORB::PublicationMultiData< T >::get(), math::LowPassFilter2pVector3f::get_cutoff_freq(), and GYRO_BASE_DEVICE_PATH.
Referenced by ADIS16477::print_info(), FXAS21002C::print_info(), ADIS16497::print_info(), ADIS16448::print_info(), BMI088_gyro::print_info(), BMI055_gyro::print_info(), L3GD20::print_info(), MPU9250::print_info(), BMI160::print_info(), MPU6000::print_info(), ICM20948::print_info(), and set_temperature().
void PX4Gyroscope::set_device_type | ( | uint8_t | devtype | ) |
Definition at line 88 of file PX4Gyroscope.cpp.
References _sensor_gyro_control_pub, _sensor_gyro_pub, sensor_gyro_s::device_id, sensor_gyro_control_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_gyro::BMI055_gyro(), BMI088_gyro::BMI088_gyro(), BMI160::BMI160(), FXAS21002C::FXAS21002C(), ICM20948::ICM20948(), L3GD20::L3GD20(), MPU6000::MPU6000(), and MPU9250::MPU9250().
|
inline |
Definition at line 58 of file PX4Gyroscope.hpp.
References _sensor_gyro_pub, sensor_gyro_s::error_count, and uORB::PublicationMultiData< T >::get().
Referenced by FXAS21002C::measure(), ADIS16448::measure(), ADIS16497::measure(), BMI055_gyro::measure(), BMI088_gyro::measure(), L3GD20::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), and ICM20948::measure().
void PX4Gyroscope::set_sample_rate | ( | unsigned | rate | ) |
Definition at line 103 of file PX4Gyroscope.cpp.
References _filter, _sample_rate, math::LowPassFilter2pVector3f::get_cutoff_freq(), and math::LowPassFilter2pVector3f::set_cutoff_frequency().
Referenced by ADIS16477::ADIS16477(), ADIS16497::ADIS16497(), ADIS16448::set_sample_rate(), FXAS21002C::set_samplerate(), and set_temperature().
|
inline |
Definition at line 59 of file PX4Gyroscope.hpp.
References _sensor_gyro_pub, uORB::PublicationMultiData< T >::get(), and sensor_gyro_s::scaling.
Referenced by ADIS16448::ADIS16448(), ADIS16477::ADIS16477(), ADIS16497::ADIS16497(), MPU6000::reset(), MPU9250::reset_mpu(), ICM20948::reset_mpu(), Sih::send_IMU(), ADIS16448::set_gyro_dyn_range(), BMI055_gyro::set_gyro_range(), BMI088_gyro::set_gyro_range(), BMI160::set_gyro_range(), FXAS21002C::set_range(), and L3GD20::set_range().
|
inline |
Definition at line 60 of file PX4Gyroscope.hpp.
References _sensor_gyro_pub, uORB::PublicationMultiData< T >::get(), hrt_abstime, print_status(), set_sample_rate(), sensor_gyro_s::temperature, and update().
Referenced by ADIS16477::measure(), FXAS21002C::measure(), ADIS16448::measure(), ADIS16497::measure(), BMI055_gyro::measure(), BMI088_gyro::measure(), L3GD20::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), ICM20948::measure(), and Sih::send_IMU().
void PX4Gyroscope::update | ( | hrt_abstime | timestamp, |
float | x, | ||
float | y, | ||
float | z | ||
) |
Definition at line 110 of file PX4Gyroscope.cpp.
References _calibration_offset, _calibration_scale, _filter, _integrator, _rotation, _sensor_gyro_control_pub, _sensor_gyro_pub, math::LowPassFilter2pVector3f::apply(), uORB::PublicationMultiData< T >::get(), hrt_absolute_time(), hrt_elapsed_time(), sensor_gyro_s::integral_dt, cdev::CDev::poll_notify(), Integrator::put(), rotate_3f(), sensor_gyro_s::scaling, sensor_gyro_control_s::timestamp, sensor_gyro_s::timestamp, sensor_gyro_control_s::timestamp_sample, uORB::PublicationMultiData< T >::update(), sensor_gyro_s::x, sensor_gyro_s::x_integral, sensor_gyro_s::x_raw, sensor_gyro_control_s::xyz, sensor_gyro_s::y, sensor_gyro_s::y_integral, sensor_gyro_s::y_raw, sensor_gyro_s::z, sensor_gyro_s::z_integral, and sensor_gyro_s::z_raw.
Referenced by ADIS16477::measure(), FXAS21002C::measure(), ADIS16448::measure(), ADIS16497::measure(), BMI055_gyro::measure(), BMI088_gyro::measure(), L3GD20::measure(), MPU9250::measure(), BMI160::measure(), MPU6000::measure(), ICM20948::measure(), Sih::send_IMU(), and set_temperature().
|
private |
Definition at line 81 of file PX4Gyroscope.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 80 of file PX4Gyroscope.hpp.
Referenced by ioctl(), print_status(), and update().
|
private |
Definition at line 83 of file PX4Gyroscope.hpp.
Referenced by print_status(), PX4Gyroscope(), and ~PX4Gyroscope().
|
private |
Definition at line 75 of file PX4Gyroscope.hpp.
Referenced by configure_filter(), print_status(), set_sample_rate(), and update().
|
private |
Definition at line 76 of file PX4Gyroscope.hpp.
Referenced by update().
|
private |
Definition at line 78 of file PX4Gyroscope.hpp.
Referenced by PX4Gyroscope(), and update().
|
private |
Definition at line 85 of file PX4Gyroscope.hpp.
Referenced by configure_filter(), print_status(), and set_sample_rate().
|
private |
Definition at line 73 of file PX4Gyroscope.hpp.
Referenced by print_status(), PX4Gyroscope(), set_device_type(), and update().
|
private |
Definition at line 72 of file PX4Gyroscope.hpp.
Referenced by ioctl(), print_status(), PX4Gyroscope(), set_device_type(), set_error_count(), set_scale(), set_temperature(), and update().