41 #include <px4_platform_common/px4_config.h> 43 #include <sys/types.h> 51 #include <px4_platform_common/getopt.h> 69 #include <lsm9ds1/LSM9DS1.hpp> 73 #define LSM9DS1_NEVER_AUTOPUBLISH_US 0 108 int _publish(
struct imu_sensor_data &
data);
110 void _update_accel_calibration();
111 void _update_gyro_calibration();
112 void _update_mag_calibration();
128 } _accel_calibration;
173 LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
174 _accel_topic(nullptr),
175 _gyro_topic(nullptr),
177 _mavlink_log_pub(nullptr),
178 _param_update_sub(-1),
179 _accel_calibration{},
247 PX4_ERR(
"sensor_accel advert fail");
257 PX4_ERR(
"sensor_gyro advert fail");
267 PX4_ERR(
"sensor_mag advert fail");
281 PX4_ERR(
"LSM9DS1 init fail: %d", ret);
288 PX4_ERR(
"LSM9DS1 start fail: %d", ret);
292 PX4_DEBUG(
"LSM9DS1 device id is: %d", m_id.dev_id);
308 PX4_ERR(
"LSM9DS1 stop fail: %d", ret);
330 for (
unsigned i = 0; i < 3; ++i) {
335 (void)sprintf(str,
"CAL_GYRO%u_ID", i);
340 PX4_ERR(
"Could not access param %s", str);
344 if ((uint32_t)device_id != m_id.dev_id) {
348 (void)sprintf(str,
"CAL_GYRO%u_XSCALE", i);
352 PX4_ERR(
"Could not access param %s", str);
355 (void)sprintf(str,
"CAL_GYRO%u_YSCALE", i);
359 PX4_ERR(
"Could not access param %s", str);
362 (void)sprintf(str,
"CAL_GYRO%u_ZSCALE", i);
366 PX4_ERR(
"Could not access param %s", str);
369 (void)sprintf(str,
"CAL_GYRO%u_XOFF", i);
373 PX4_ERR(
"Could not access param %s", str);
376 (void)sprintf(str,
"CAL_GYRO%u_YOFF", i);
380 PX4_ERR(
"Could not access param %s", str);
383 (void)sprintf(str,
"CAL_GYRO%u_ZOFF", i);
387 PX4_ERR(
"Could not access param %s", str);
405 for (
unsigned i = 0; i < 3; ++i) {
410 (void)sprintf(str,
"CAL_ACC%u_ID", i);
415 PX4_ERR(
"Could not access param %s", str);
419 if ((uint32_t)device_id != m_id.dev_id) {
423 (void)sprintf(str,
"CAL_ACC%u_XSCALE", i);
427 PX4_ERR(
"Could not access param %s", str);
430 (void)sprintf(str,
"CAL_ACC%u_YSCALE", i);
434 PX4_ERR(
"Could not access param %s", str);
437 (void)sprintf(str,
"CAL_ACC%u_ZSCALE", i);
441 PX4_ERR(
"Could not access param %s", str);
444 (void)sprintf(str,
"CAL_ACC%u_XOFF", i);
448 PX4_ERR(
"Could not access param %s", str);
451 (void)sprintf(str,
"CAL_ACC%u_YOFF", i);
455 PX4_ERR(
"Could not access param %s", str);
458 (void)sprintf(str,
"CAL_ACC%u_ZOFF", i);
462 PX4_ERR(
"Could not access param %s", str);
485 for (
unsigned i = 0; i < 3; ++i) {
490 (void)sprintf(str,
"CAL_MAG%u_ID", i);
495 PX4_ERR(
"Could not access param %s", str);
499 if ((uint32_t)device_id != m_id.dev_id) {
503 (void)sprintf(str,
"CAL_MAG%u_XSCALE", i);
507 PX4_ERR(
"Could not access param %s", str);
510 (void)sprintf(str,
"CAL_MAG%u_YSCALE", i);
514 PX4_ERR(
"Could not access param %s", str);
517 (void)sprintf(str,
"CAL_MAG%u_ZSCALE", i);
521 PX4_ERR(
"Could not access param %s", str);
524 (void)sprintf(str,
"CAL_MAG%u_XOFF", i);
528 PX4_ERR(
"Could not access param %s", str);
531 (void)sprintf(str,
"CAL_MAG%u_YOFF", i);
535 PX4_ERR(
"Could not access param %s", str);
538 (void)sprintf(str,
"CAL_MAG%u_ZOFF", i);
542 PX4_ERR(
"Could not access param %s", str);
573 uint32_t integral_dt_unused;
585 vec_integrated_unused,
598 vec_integrated_unused,
626 mag_report.timestamp = accel_report.
timestamp;
627 mag_report.is_external =
false;
637 mag_report.scaling = -1.0f;
638 mag_report.device_id = m_id.dev_id;
642 gyro_report.
x_raw = 0;
643 gyro_report.
y_raw = 0;
644 gyro_report.
z_raw = 0;
646 accel_report.
x_raw = 0;
647 accel_report.
y_raw = 0;
648 accel_report.
z_raw = 0;
651 mag_report.x_raw = 0;
652 mag_report.y_raw = 0;
653 mag_report.z_raw = 0;
664 gyro_report.
x = gyro_val_filt(0);
665 gyro_report.
y = gyro_val_filt(1);
666 gyro_report.
z = gyro_val_filt(2);
668 accel_report.
x = accel_val_filt(0);
669 accel_report.
y = accel_val_filt(1);
670 accel_report.
z = accel_val_filt(2);
681 mag_report.x = mag_val(0);
682 mag_report.y = mag_val(1);
683 mag_report.z = mag_val(2);
695 if (!(m_pub_blocked)) {
716 if (threshold_reached && due_to_report) {
718 "High accelerations, range exceeded %llu times",
719 data.accel_range_hit_counter);
747 if (g_dev ==
nullptr) {
748 PX4_ERR(
"failed instantiating DfLsm9ds1Wrapper object");
752 int ret = g_dev->
start();
755 PX4_ERR(
"DfLsm9ds1Wrapper start failed");
761 DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
764 DF_LOG_INFO(
"Error: unable to obtain a valid handle for the receiver at: %s (%d)",
765 IMU_DEVICE_ACC_GYRO, h.getError());
769 DevMgr::releaseHandle(h);
771 DevMgr::getHandle(IMU_DEVICE_MAG, h);
774 DF_LOG_INFO(
"Error: unable to obtain a valid handle for the receiver at: %s (%d)",
775 IMU_DEVICE_MAG, h.getError());
779 DevMgr::releaseHandle(h);
785 if (g_dev ==
nullptr) {
786 PX4_ERR(
"driver not running");
790 int ret = g_dev->
stop();
793 PX4_ERR(
"driver could not be stopped");
808 if (g_dev ==
nullptr) {
809 PX4_ERR(
"driver not running");
813 PX4_INFO(
"state @ %p", g_dev);
822 PX4_INFO(
"Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'");
823 PX4_INFO(
"options:");
824 PX4_INFO(
" -R rotation");
837 const char *myoptarg = NULL;
840 while ((ch = px4_getopt(argc, argv,
"R:", &myoptind, &myoptarg)) != EOF) {
843 rotation = (
enum Rotation)atoi(myoptarg);
857 const char *verb = argv[myoptind];
859 if (!strcmp(verb,
"start_without_mag")) {
863 else if (!strcmp(verb,
"start")) {
867 else if (!strcmp(verb,
"stop")) {
871 else if (!strcmp(verb,
"info")) {
uint64_t _last_accel_range_hit_count
int stop()
Stop the driver.
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
matrix::Dcmf _rotation_matrix
Gyroscope driver interface.
int info()
Print a little info about the driver.
measure the time elapsed performing an event
struct DfLsm9ds1Wrapper::mag_calibration_s _mag_calibration
perf_counter_t _fifo_overflow_counter
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
void usage(const char *reason)
Print the correct usage.
perf_counter_t _error_counter
count the number of times an event occurs
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
perf_counter_t _fifo_corruption_counter
matrix::Vector3f get_and_filtered(bool reset, uint32_t &integral_dt, matrix::Vector3f &filtered_val)
Get the current integral and reset the integrator if needed.
Global flash based parameter store.
int orb_subscribe(const struct orb_metadata *meta)
int _gyro_orb_class_instance
perf_counter_t _gyro_range_hit_counter
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void _update_gyro_calibration()
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void _update_accel_calibration()
int stop()
Stop automatic measurement.
Rotation
Enum for board and external compass rotations.
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
int _publish(struct imu_sensor_data &data)
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
orb_advert_t _accel_topic
void perf_end(perf_counter_t handle)
End a performance event.
void usage()
Prints info about the driver argument usage.
bool put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
Put an item into the integral but provide an interval instead of a timestamp.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
void _update_mag_calibration()
struct DfLsm9ds1Wrapper::gyro_calibration_s _gyro_calibration
struct DfLsm9ds1Wrapper::accel_calibration_s _accel_calibration
perf_counter_t _publish_perf
DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation)
int orb_check(int handle, bool *updated)
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
orb_advert_t _mavlink_log_pub
void info()
Print some debug info.
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
void perf_set_count(perf_counter_t handle, uint64_t count)
Set a counter.
hrt_abstime _last_accel_range_hit_time
int _mag_orb_class_instance
__EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[])
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
#define LSM9DS1_NEVER_AUTOPUBLISH_US
perf_counter_t _read_counter
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t _accel_range_hit_counter
int start()
Start automatic measurement.
int start(bool mag_enabled, enum Rotation rotation)
int _accel_orb_class_instance
Performance measuring tools.