|
PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <LSM303AGR.hpp>
Public Member Functions | |
| LSM303AGR (int bus, const char *path, uint32_t device, enum Rotation rotation) | |
| virtual | ~LSM303AGR () |
| virtual int | init () |
| virtual int | ioctl (struct file *filp, int cmd, unsigned long arg) |
| void | print_info () |
| Diagnostics - print some basic information about the driver. More... | |
Protected Member Functions | |
| virtual int | probe () |
Private Member Functions | |
| void | start () |
| Start automatic measurement. More... | |
| void | stop () |
| Stop automatic measurement. More... | |
| int | reset () |
| Reset chip. More... | |
| bool | self_test () |
| void | measure () |
| Issue a measurement command. More... | |
| int | collect () |
| Collect the result of the most recent measurement. More... | |
| void | Run () override |
| Perform a poll cycle; collect from the previous measurement and start a new one. More... | |
| uint8_t | read_reg (unsigned reg) |
| Read a register from the LSM303AGR. More... | |
| void | write_reg (unsigned reg, uint8_t value) |
| Write a register in the LSM303AGR. More... | |
| LSM303AGR (const LSM303AGR &) | |
| LSM303AGR | operator= (const LSM303AGR &) |
Private Attributes | |
| bool | _collect_phase {false} |
| unsigned | _measure_interval {0} |
| unsigned | _call_mag_interval {0} |
| mag_calibration_s | _mag_scale {} |
| int | _class_instance {-1} |
| unsigned | _mag_samplerate {100} |
| perf_counter_t | _mag_sample_perf |
| perf_counter_t | _bad_registers |
| perf_counter_t | _bad_values |
| enum Rotation | _rotation |
| orb_advert_t | _mag_topic {nullptr} |
| int | _mag_orb_class_instance {-1} |
Static Private Attributes | |
| static constexpr float | _mag_range_scale {1.5f / 1000.0f} |
| static constexpr float | _mag_range_ga {49.152f} |
Definition at line 91 of file LSM303AGR.hpp.
| LSM303AGR::LSM303AGR | ( | int | bus, |
| const char * | path, | ||
| uint32_t | device, | ||
| enum Rotation | rotation | ||
| ) |
Definition at line 64 of file LSM303AGR.cpp.
References _mag_scale, DRV_MAG_DEVTYPE_LSM303AGR, 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.
|
virtual |
Definition at line 84 of file LSM303AGR.cpp.
References _bad_registers, _bad_values, _class_instance, _mag_sample_perf, MAG_BASE_DEVICE_PATH, perf_free(), and stop().
|
private |
|
private |
Collect the result of the most recent measurement.
Definition at line 417 of file LSM303AGR.cpp.
References _bad_registers, _bad_values, _mag_orb_class_instance, _mag_range_scale, _mag_sample_perf, _mag_scale, _mag_topic, _rotation, DEVICE_DEBUG, hrt_absolute_time(), mag_report, OK, orb_advertise_multi(), ORB_ID, ORB_PRIO_HIGH, ORB_PRIO_MAX, orb_publish(), OUTX_H_REG_M, OUTX_L_REG_M, OUTY_H_REG_M, OUTY_L_REG_M, OUTZ_H_REG_M, OUTZ_L_REG_M, perf_begin(), perf_end(), perf_event_count(), read_reg(), rotate_3f(), status, STATUS_REG_M, STATUS_REG_M_Zyxda, 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 Run().
|
virtual |
Definition at line 100 of file LSM303AGR.cpp.
References _class_instance, ToneAlarmInterface::init(), MAG_BASE_DEVICE_PATH, measure(), OK, reset(), and self_test().
Referenced by lsm303agr::start().
|
virtual |
Definition at line 243 of file LSM303AGR.cpp.
References _mag_scale, _measure_interval, CONVERSION_INTERVAL, MAGIOCGEXTERNAL, MAGIOCGSCALE, MAGIOCSSCALE, OK, reset(), SENSOR_POLLRATE_DEFAULT, SENSORIOCRESET, SENSORIOCSPOLLRATE, and start().
|
private |
Issue a measurement command.
Definition at line 408 of file LSM303AGR.cpp.
References CFG_REG_A_M, CFG_REG_A_M_MD0, CFG_REG_A_M_ODR0, CFG_REG_A_M_ODR1, and write_reg().
Referenced by init(), and Run().
| void LSM303AGR::print_info | ( | ) |
Diagnostics - print some basic information about the driver.
Definition at line 479 of file LSM303AGR.cpp.
References _bad_registers, _bad_values, _mag_sample_perf, and perf_print_counter().
Referenced by lsm303agr::info().
|
protectedvirtual |
Definition at line 230 of file LSM303AGR.cpp.
References LSM303AGR_WHO_AM_I_M, OK, read_reg(), and WHO_AM_I_M.
|
private |
Read a register from the LSM303AGR.
| The | register to read. |
Definition at line 319 of file LSM303AGR.cpp.
References DIR_READ.
Referenced by collect(), probe(), and self_test().
|
private |
Reset chip.
Resets the chip and measurements ranges, but not scale and offset.
Definition at line 125 of file LSM303AGR.cpp.
References CFG_REG_A_M, CFG_REG_A_M_MD0, CFG_REG_A_M_ODR0, CFG_REG_A_M_ODR1, CFG_REG_B_M, CFG_REG_B_M_OFF_CANC, CFG_REG_B_M_OFF_LPF, CFG_REG_C_M, CFG_REG_C_M_I2C_DIS, 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 363 of file LSM303AGR.cpp.
References _collect_phase, _measure_interval, collect(), CONVERSION_INTERVAL, DEVICE_DEBUG, measure(), OK, and start().
|
private |
Definition at line 141 of file LSM303AGR.cpp.
References matrix::abs(), CFG_REG_A_M, CFG_REG_B_M, CFG_REG_C_M, OUTX_H_REG_M, OUTX_L_REG_M, OUTY_H_REG_M, OUTY_L_REG_M, OUTZ_H_REG_M, OUTZ_L_REG_M, read_reg(), STATUS_REG_M, and write_reg().
Referenced by init().
|
private |
Start automatic measurement.
Definition at line 343 of file LSM303AGR.cpp.
References _collect_phase.
Referenced by ioctl(), and Run().
|
private |
Stop automatic measurement.
Definition at line 353 of file LSM303AGR.cpp.
References _measure_interval.
Referenced by ~LSM303AGR().
|
private |
Write a register in the LSM303AGR.
| reg | The register to write. |
| value | The new value to write. |
Definition at line 332 of file LSM303AGR.cpp.
References DIR_WRITE.
Referenced by measure(), reset(), and self_test().
|
private |
Definition at line 127 of file LSM303AGR.hpp.
Referenced by collect(), print_info(), and ~LSM303AGR().
|
private |
Definition at line 128 of file LSM303AGR.hpp.
Referenced by collect(), print_info(), and ~LSM303AGR().
|
private |
Definition at line 115 of file LSM303AGR.hpp.
|
private |
Definition at line 122 of file LSM303AGR.hpp.
Referenced by init(), and ~LSM303AGR().
|
private |
Definition at line 111 of file LSM303AGR.hpp.
|
private |
Definition at line 133 of file LSM303AGR.hpp.
Referenced by collect().
|
staticprivate |
Definition at line 120 of file LSM303AGR.hpp.
|
staticprivate |
Definition at line 119 of file LSM303AGR.hpp.
Referenced by collect().
|
private |
Definition at line 126 of file LSM303AGR.hpp.
Referenced by collect(), print_info(), and ~LSM303AGR().
|
private |
Definition at line 124 of file LSM303AGR.hpp.
|
private |
Definition at line 117 of file LSM303AGR.hpp.
Referenced by collect(), ioctl(), and LSM303AGR().
|
private |
Definition at line 132 of file LSM303AGR.hpp.
Referenced by collect().
|
private |
Definition at line 113 of file LSM303AGR.hpp.
|
private |
Definition at line 130 of file LSM303AGR.hpp.
Referenced by collect().