41 #include <px4_platform_common/px4_config.h> 43 #include <sys/types.h> 51 #include <px4_platform_common/getopt.h> 69 #include <mpu9250/MPU9250.hpp> 72 #define MPU9250_ACCEL_DEFAULT_RATE 1000 73 #define MPU9250_GYRO_DEFAULT_RATE 1000 75 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 76 #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 78 #define MPU9250_PUB_RATE 280 113 int _publish(
struct imu_sensor_data &
data);
115 void _update_accel_calibration();
116 void _update_gyro_calibration();
117 void _update_mag_calibration();
134 } _accel_calibration;
186 MPU9250(IMU_DEVICE_PATH, mag_enabled),
187 _accel_topic(nullptr),
188 _gyro_topic(nullptr),
190 _mavlink_log_pub(nullptr),
191 _param_update_sub(-1),
192 _accel_calibration{},
253 PX4_ERR(
"IMU_ACCEL_CUTOFF param invalid");
265 PX4_ERR(
"IMU_GYRO_CUTOFF param invalid");
293 PX4_ERR(
"sensor_accel advert fail");
303 PX4_ERR(
"sensor_gyro advert fail");
319 PX4_ERR(
"MPU9250 init fail: %d", ret);
326 PX4_ERR(
"MPU9250 start fail: %d", ret);
330 PX4_DEBUG(
"MPU9250 device id is: %d", m_id.dev_id);
346 PX4_ERR(
"MPU9250 stop fail: %d", ret);
372 for (
unsigned i = 0; i < 3; ++i) {
377 (void)sprintf(str,
"CAL_GYRO%u_ID", i);
382 PX4_ERR(
"Could not access param %s", str);
386 if ((uint32_t)device_id != m_id.dev_id) {
390 (void)sprintf(str,
"CAL_GYRO%u_XSCALE", i);
394 PX4_ERR(
"Could not access param %s", str);
397 (void)sprintf(str,
"CAL_GYRO%u_YSCALE", i);
401 PX4_ERR(
"Could not access param %s", str);
404 (void)sprintf(str,
"CAL_GYRO%u_ZSCALE", i);
408 PX4_ERR(
"Could not access param %s", str);
411 (void)sprintf(str,
"CAL_GYRO%u_XOFF", i);
415 PX4_ERR(
"Could not access param %s", str);
418 (void)sprintf(str,
"CAL_GYRO%u_YOFF", i);
422 PX4_ERR(
"Could not access param %s", str);
425 (void)sprintf(str,
"CAL_GYRO%u_ZOFF", i);
429 PX4_ERR(
"Could not access param %s", str);
447 for (
unsigned i = 0; i < 3; ++i) {
452 (void)sprintf(str,
"CAL_ACC%u_ID", i);
457 PX4_ERR(
"Could not access param %s", str);
461 if ((uint32_t)device_id != m_id.dev_id) {
465 (void)sprintf(str,
"CAL_ACC%u_XSCALE", i);
469 PX4_ERR(
"Could not access param %s", str);
472 (void)sprintf(str,
"CAL_ACC%u_YSCALE", i);
476 PX4_ERR(
"Could not access param %s", str);
479 (void)sprintf(str,
"CAL_ACC%u_ZSCALE", i);
483 PX4_ERR(
"Could not access param %s", str);
486 (void)sprintf(str,
"CAL_ACC%u_XOFF", i);
490 PX4_ERR(
"Could not access param %s", str);
493 (void)sprintf(str,
"CAL_ACC%u_YOFF", i);
497 PX4_ERR(
"Could not access param %s", str);
500 (void)sprintf(str,
"CAL_ACC%u_ZOFF", i);
504 PX4_ERR(
"Could not access param %s", str);
527 for (
unsigned i = 0; i < 3; ++i) {
532 (void)sprintf(str,
"CAL_MAG%u_ID", i);
537 PX4_ERR(
"Could not access param %s", str);
541 if ((uint32_t)device_id != m_id.dev_id) {
545 (void)sprintf(str,
"CAL_MAG%u_XSCALE", i);
549 PX4_ERR(
"Could not access param %s", str);
552 (void)sprintf(str,
"CAL_MAG%u_YSCALE", i);
556 PX4_ERR(
"Could not access param %s", str);
559 (void)sprintf(str,
"CAL_MAG%u_ZSCALE", i);
563 PX4_ERR(
"Could not access param %s", str);
566 (void)sprintf(str,
"CAL_MAG%u_XOFF", i);
570 PX4_ERR(
"Could not access param %s", str);
573 (void)sprintf(str,
"CAL_MAG%u_YOFF", i);
577 PX4_ERR(
"Could not access param %s", str);
580 (void)sprintf(str,
"CAL_MAG%u_ZOFF", i);
584 PX4_ERR(
"Could not access param %s", str);
625 float xraw_f = data.accel_m_s2_x;
626 float yraw_f = data.accel_m_s2_y;
627 float zraw_f = data.accel_m_s2_z;
634 accel_report.
x_raw = (int16_t)(xraw_f * 1000);
635 accel_report.
y_raw = (int16_t)(yraw_f * 1000);
636 accel_report.
z_raw = (int16_t)(zraw_f * 1000);
657 xraw_f = data.gyro_rad_s_x;
658 yraw_f = data.gyro_rad_s_y;
659 zraw_f = data.gyro_rad_s_z;
666 gyro_report.
x_raw = (int16_t)(xraw_f * 1000);
667 gyro_report.
y_raw = (int16_t)(yraw_f * 1000);
668 gyro_report.
z_raw = (int16_t)(zraw_f * 1000);
691 if (!sensor_notify) {
717 mag_report.timestamp = accel_report.
timestamp;
718 mag_report.is_external =
false;
720 mag_report.scaling = -1.0f;
721 mag_report.device_id = m_id.dev_id;
723 xraw_f = data.mag_ga_x;
724 yraw_f = data.mag_ga_y;
725 zraw_f = data.mag_ga_z;
731 mag_report.x_raw = xraw_f * 1000;
732 mag_report.y_raw = yraw_f * 1000;
733 mag_report.z_raw = zraw_f * 1000;
741 if (!(m_pub_blocked) && sensor_notify) {
768 if (threshold_reached && due_to_report) {
770 "High accelerations, range exceeded %llu times",
771 data.accel_range_hit_counter);
799 if (g_dev ==
nullptr) {
800 PX4_ERR(
"failed instantiating DfMpu9250Wrapper object");
804 int ret = g_dev->
start();
807 PX4_ERR(
"DfMpu9250Wrapper start failed");
813 DevMgr::getHandle(IMU_DEVICE_PATH, h);
816 DF_LOG_INFO(
"Error: unable to obtain a valid handle for the receiver at: %s (%d)",
817 IMU_DEVICE_PATH, h.getError());
821 DevMgr::releaseHandle(h);
828 if (g_dev ==
nullptr) {
829 PX4_ERR(
"driver not running");
833 int ret = g_dev->
stop();
836 PX4_ERR(
"driver could not be stopped");
851 if (g_dev ==
nullptr) {
852 PX4_ERR(
"driver not running");
856 PX4_INFO(
"state @ %p", g_dev);
865 PX4_INFO(
"Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
866 PX4_INFO(
"options:");
867 PX4_INFO(
" -R rotation");
880 const char *myoptarg = NULL;
883 while ((ch = px4_getopt(argc, argv,
"R:", &myoptind, &myoptarg)) != EOF) {
886 rotation = (
enum Rotation)atoi(myoptarg);
900 const char *verb = argv[myoptind];
902 if (!strcmp(verb,
"start_without_mag")) {
906 else if (!strcmp(verb,
"start")) {
910 else if (!strcmp(verb,
"stop")) {
914 else if (!strcmp(verb,
"info")) {
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
math::LowPassFilter2p _gyro_filter_y
int _gyro_orb_class_instance
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
Accelerometer driver interface.
hrt_abstime _last_accel_range_hit_time
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Gyroscope driver interface.
perf_counter_t _fifo_corruption_counter
void info()
Print some debug info.
measure the time elapsed performing an event
int _accel_orb_class_instance
struct DfMpu9250Wrapper::accel_calibration_s _accel_calibration
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
rotate a 3 element float vector in-place
void usage(const char *reason)
Print the correct usage.
void set_cutoff_frequency(float sample_freq, float cutoff_freq)
math::LowPassFilter2p _accel_filter_z
count the number of times an event occurs
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ
perf_counter_t _fifo_overflow_counter
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
Put an item into the integral.
Global flash based parameter store.
math::LowPassFilter2p _gyro_filter_x
struct DfMpu9250Wrapper::mag_calibration_s _mag_calibration
perf_counter_t _publish_perf
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void perf_free(perf_counter_t handle)
Free a counter.
struct DfMpu9250Wrapper::gyro_calibration_s _gyro_calibration
#define MPU9250_GYRO_DEFAULT_RATE
uint64_t _last_accel_range_hit_count
perf_counter_t _error_counter
Rotation
Enum for board and external compass rotations.
void _update_accel_calibration()
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
int stop()
Stop automatic measurement.
int stop()
Stop the driver.
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void perf_end(perf_counter_t handle)
End a performance event.
math::LowPassFilter2p _accel_filter_y
__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.
perf_counter_t _mag_fifo_overflow_counter
float apply(float sample)
Add a new raw value to the filter.
int info()
Print a little info about the driver.
void _update_gyro_calibration()
int start()
Start automatic measurement.
int _publish(struct imu_sensor_data &data)
int orb_check(int handle, bool *updated)
void usage()
Prints info about the driver argument usage.
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
perf_counter_t _read_counter
DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation)
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ
#define MPU9250_ACCEL_DEFAULT_RATE
orb_advert_t _mavlink_log_pub
__EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[])
void perf_set_count(perf_counter_t handle, uint64_t count)
Set a counter.
math::LowPassFilter2p _accel_filter_x
perf_counter_t _accel_range_hit_counter
orb_advert_t _accel_topic
perf_counter_t _gyro_range_hit_counter
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
void perf_begin(perf_counter_t handle)
Begin a performance event.
math::LowPassFilter2p _gyro_filter_z
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
int _mag_orb_class_instance
void _update_mag_calibration()
int start(bool mag_enabled, enum Rotation rotation)
Performance measuring tools.