39 #include <px4_platform_common/px4_config.h> 41 #include <sys/types.h> 49 #include <px4_platform_common/getopt.h> 66 #include <mpu6050/MPU6050.hpp> 70 #define MPU6050_NEVER_AUTOPUBLISH_US 0 105 int _publish(
struct imu_sensor_data &
data);
107 void _update_accel_calibration();
108 void _update_gyro_calibration();
124 } _accel_calibration;
159 MPU6050(IMU_DEVICE_PATH),
160 _accel_topic(nullptr),
161 _gyro_topic(nullptr),
162 _mavlink_log_pub(nullptr),
163 _param_update_sub(-1),
164 _accel_calibration{},
223 PX4_ERR(
"MPU6050 init fail: %d", ret);
230 PX4_ERR(
"MPU6050 start fail: %d", ret);
234 PX4_DEBUG(
"MPU6050 device id is: %d", m_id.dev_id);
249 PX4_ERR(
"MPU6050 stop fail: %d", ret);
271 for (
unsigned i = 0; i < 3; ++i) {
276 (void)sprintf(str,
"CAL_GYRO%u_ID", i);
281 PX4_ERR(
"Could not access param %s", str);
285 if ((uint32_t)device_id != m_id.dev_id) {
289 (void)sprintf(str,
"CAL_GYRO%u_XSCALE", i);
293 PX4_ERR(
"Could not access param %s", str);
296 (void)sprintf(str,
"CAL_GYRO%u_YSCALE", i);
300 PX4_ERR(
"Could not access param %s", str);
303 (void)sprintf(str,
"CAL_GYRO%u_ZSCALE", i);
307 PX4_ERR(
"Could not access param %s", str);
310 (void)sprintf(str,
"CAL_GYRO%u_XOFF", i);
314 PX4_ERR(
"Could not access param %s", str);
317 (void)sprintf(str,
"CAL_GYRO%u_YOFF", i);
321 PX4_ERR(
"Could not access param %s", str);
324 (void)sprintf(str,
"CAL_GYRO%u_ZOFF", i);
328 PX4_ERR(
"Could not access param %s", str);
346 for (
unsigned i = 0; i < 3; ++i) {
351 (void)sprintf(str,
"CAL_ACC%u_ID", i);
356 PX4_ERR(
"Could not access param %s", str);
360 if ((uint32_t)device_id != m_id.dev_id) {
364 (void)sprintf(str,
"CAL_ACC%u_XSCALE", i);
368 PX4_ERR(
"Could not access param %s", str);
371 (void)sprintf(str,
"CAL_ACC%u_YSCALE", i);
375 PX4_ERR(
"Could not access param %s", str);
378 (void)sprintf(str,
"CAL_ACC%u_ZSCALE", i);
382 PX4_ERR(
"Could not access param %s", str);
385 (void)sprintf(str,
"CAL_ACC%u_XOFF", i);
389 PX4_ERR(
"Could not access param %s", str);
392 (void)sprintf(str,
"CAL_ACC%u_YOFF", i);
396 PX4_ERR(
"Could not access param %s", str);
399 (void)sprintf(str,
"CAL_ACC%u_ZOFF", i);
403 PX4_ERR(
"Could not access param %s", str);
436 uint32_t integral_dt_unused;
438 matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
449 vec_integrated_unused,
452 matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
463 vec_integrated_unused,
468 if (!data.is_last_fifo_sample) {
505 gyro_report.
x_raw = 0;
506 gyro_report.
y_raw = 0;
507 gyro_report.
z_raw = 0;
509 accel_report.
x_raw = 0;
510 accel_report.
y_raw = 0;
511 accel_report.
z_raw = 0;
521 gyro_report.
x = gyro_val_filt(0);
522 gyro_report.
y = gyro_val_filt(1);
523 gyro_report.
z = gyro_val_filt(2);
525 accel_report.
x = accel_val_filt(0);
526 accel_report.
y = accel_val_filt(1);
527 accel_report.
z = accel_val_filt(2);
538 if (!(m_pub_blocked)) {
562 if (threshold_reached && due_to_report) {
564 "High accelerations, range exceeded %llu times",
565 data.accel_range_hit_counter);
593 if (g_dev ==
nullptr) {
594 PX4_ERR(
"failed instantiating DfMPU6050Wrapper object");
598 int ret = g_dev->
start();
601 PX4_ERR(
"DfMPU6050Wrapper start failed");
607 DevMgr::getHandle(IMU_DEVICE_PATH, h);
610 DF_LOG_INFO(
"Error: unable to obtain a valid handle for the receiver at: %s (%d)",
611 IMU_DEVICE_PATH, h.getError());
615 DevMgr::releaseHandle(h);
622 if (g_dev ==
nullptr) {
623 PX4_ERR(
"driver not running");
627 int ret = g_dev->
stop();
630 PX4_ERR(
"driver could not be stopped");
645 if (g_dev ==
nullptr) {
646 PX4_ERR(
"driver not running");
650 PX4_INFO(
"state @ %p", g_dev);
659 PX4_INFO(
"Usage: df_mpu6050_wrapper 'start', 'info', 'stop'");
660 PX4_INFO(
"options:");
661 PX4_INFO(
" -R rotation");
674 const char *myoptarg = NULL;
677 while ((ch = px4_getopt(argc, argv,
"R:", &myoptind, &myoptarg)) != EOF) {
680 rotation = (
enum Rotation)atoi(myoptarg);
694 const char *verb = argv[myoptind];
696 if (!strcmp(verb,
"start")) {
700 else if (!strcmp(verb,
"stop")) {
704 else if (!strcmp(verb,
"info")) {
perf_counter_t _accel_range_hit_counter
orb_advert_t _mavlink_log_pub
int start(enum Rotation rotation)
#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)
perf_counter_t _error_counter
perf_counter_t _read_counter
Gyroscope driver interface.
measure the time elapsed performing an event
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
uint64_t _last_accel_range_hit_count
void usage(const char *reason)
Print the correct usage.
int stop()
Stop the driver.
struct DfMPU6050Wrapper::gyro_calibration_s _gyro_calibration
void usage()
Prints info about the driver argument usage.
count the number of times an event occurs
static int32_t device_id[max_accel_sens]
void info()
Print some debug info.
High-resolution timer with callouts and timekeeping.
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.
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.
perf_counter_t _fifo_corruption_counter
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
__EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[])
void perf_free(perf_counter_t handle)
Free a counter.
int _gyro_orb_class_instance
void init()
Activates/configures the hardware registers.
int _accel_orb_class_instance
Rotation
Enum for board and external compass rotations.
perf_counter_t _fifo_overflow_counter
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
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.
hrt_abstime _last_accel_range_hit_time
struct DfMPU6050Wrapper::accel_calibration_s _accel_calibration
__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.
int start()
Start automatic measurement.
int stop()
Stop automatic measurement.
#define MPU6050_NEVER_AUTOPUBLISH_US
perf_counter_t _gyro_range_hit_counter
void _update_gyro_calibration()
int info()
Print a little info about the driver.
void _update_accel_calibration()
int orb_check(int handle, bool *updated)
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
int _publish(struct imu_sensor_data &data)
__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.
perf_counter_t _publish_perf
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
matrix::Dcmf _rotation_matrix
void perf_begin(perf_counter_t handle)
Begin a performance event.
DfMPU6050Wrapper(enum Rotation rotation)
orb_advert_t _accel_topic
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.