diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake index ed6cd83c16..c3cbd6e475 100644 --- a/cmake/configs/posix_eagle_legacy_driver_default.cmake +++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake @@ -1,6 +1,6 @@ include(posix/px4_impl_posix) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon") @@ -12,7 +12,7 @@ set(CONFIG_SHMEM "1") # or if it is for the Snapdragon. add_definitions( -D__PX4_POSIX_EAGLE - -D__USING_SNAPDRAGON_LEGACY_DRIVER + -D__USING_SNAPDRAGON_LEGACY_DRIVER ) set(config_module_list @@ -48,6 +48,8 @@ set(config_module_list modules/logger modules/simulator modules/commander + modules/navigator + modules/load_mon lib/controllib lib/mathlib diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake index ad73ffb12d..2ccd5123d5 100644 --- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake +++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake @@ -16,6 +16,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexa add_definitions( -D__USING_SNAPDRAGON_LEGACY_DRIVER + -D__PX4_QURT + -D__PX4_QURT_EAGLE ) set(config_module_list @@ -26,8 +28,6 @@ set(config_module_list modules/sensors platforms/posix/drivers/df_mpu9250_wrapper platforms/posix/drivers/df_bmp280_wrapper - platforms/posix/drivers/df_hmc5883_wrapper - platforms/posix/drivers/df_trone_wrapper # # System commands @@ -37,7 +37,6 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - #modules/attitude_estimator_ekf modules/ekf_att_pos_estimator modules/attitude_estimator_q modules/position_estimator_inav @@ -72,7 +71,6 @@ set(config_module_list # platforms/qurt/fc_addon/rc_receiver platforms/qurt/fc_addon/uart_esc - #platforms/qurt/fc_addon/mpu_spi # # Libraries @@ -105,6 +103,4 @@ set(config_module_list set(config_df_driver_list mpu9250 bmp280 - hmc5883 - trone ) diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-calib.config index cabe21c4a0..74685189de 100644 --- a/posix-configs/eagle/200qx/px4-calib.config +++ b/posix-configs/eagle/200qx/px4-calib.config @@ -59,9 +59,9 @@ param set MC_ROLLRATE_D 0.0001 param set ATT_W_MAG 0.00 param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENM 4 -param set MPU_ACC_LPF_ENM 4 -param set MPU_SAMPLE_R_ENM 2 +param set MPU_GYR_LPF_ENUM 4 +param set MPU_ACC_LPF_ENUM 4 +param set MPU_SMPRATE_ENUM 2 sleep 1 mpu9x50 start -D /dev/spi-1 uart_esc start -D /dev/tty-2 diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config index f07aa5d026..fa8ead0e46 100644 --- a/posix-configs/eagle/200qx/px4-flight-v1-board.config +++ b/posix-configs/eagle/200qx/px4-flight-v1-board.config @@ -51,15 +51,15 @@ param set RC6_REV 1 param set ATT_W_MAG 0.00 param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 6 -param set MPU_GYRO_LPF_ENM 4 -param set MPU_ACC_LPF_ENM 4 -param set MPU_SAMPLE_R_ENM 2 +param set MPU_GYR_LPF_ENUM 4 +param set MPU_ACC_LPF_ENUM 4 +param set MPU_SMPRATE_ENUM 2 param set UART_ESC_MODEL 0 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 2 -param set UART_ESC_MOTOR2 4 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 +param set UART_ESC_BAUDRAT 250000 +param set UART_ESC_PX4MOT1 2 +param set UART_ESC_PX4MOT2 4 +param set UART_ESC_PX4MOT3 1 +param set UART_ESC_PX4MOT4 3 sleep 1 commander start rc_receiver start -D /dev/tty-1 diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config index 90fb8c2a0c..c201cee7fc 100644 --- a/posix-configs/eagle/200qx/px4-flight-v2-board.config +++ b/posix-configs/eagle/200qx/px4-flight-v2-board.config @@ -51,15 +51,15 @@ param set RC6_REV 1 param set ATT_W_MAG 0.00 param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENM 4 -param set MPU_ACC_LPF_ENM 4 -param set MPU_SAMPLE_R_ENM 2 +param set MPU_GYR_LPF_ENUM 4 +param set MPU_ACC_LPF_ENUM 4 +param set MPU_SMPRATE_ENUM 2 param set UART_ESC_MODEL 0 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 2 -param set UART_ESC_MOTOR2 4 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 +param set UART_ESC_BAUDRAT 250000 +param set UART_ESC_PX4MOT1 2 +param set UART_ESC_PX4MOT2 4 +param set UART_ESC_PX4MOT3 1 +param set UART_ESC_PX4MOT4 3 sleep 1 commander start rc_receiver start -D /dev/tty-1 diff --git a/posix-configs/eagle/210qc/mainapp-calib.config b/posix-configs/eagle/210qc/mainapp-calib.config deleted file mode 100644 index 7130d85ded..0000000000 --- a/posix-configs/eagle/210qc/mainapp-calib.config +++ /dev/null @@ -1,8 +0,0 @@ -uorb start -muorb start -mavlink start -u 14556 -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete -commander start diff --git a/posix-configs/eagle/210qc/mainapp-flight.config b/posix-configs/eagle/210qc/mainapp-flight.config deleted file mode 100644 index b511bb1121..0000000000 --- a/posix-configs/eagle/210qc/mainapp-flight.config +++ /dev/null @@ -1,7 +0,0 @@ -uorb start -muorb start -mavlink start -u 14556 -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete diff --git a/posix-configs/eagle/210qc/px4-calib.config b/posix-configs/eagle/210qc/px4-calib.config deleted file mode 100644 index cabe21c4a0..0000000000 --- a/posix-configs/eagle/210qc/px4-calib.config +++ /dev/null @@ -1,72 +0,0 @@ -uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set RC_RECEIVER_TYPE 1 -rc_receiver start -D /dev/tty-1 -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -sleep 1 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2​ -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -sensors start -param set MC_YAW_P 7.0 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.0001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.0001 -param set ATT_W_MAG 0.00 -param set PE_MAG_NOISE 1.0f -param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENM 4 -param set MPU_ACC_LPF_ENM 4 -param set MPU_SAMPLE_R_ENM 2 -sleep 1 -mpu9x50 start -D /dev/spi-1 -uart_esc start -D /dev/tty-2 -csr_gps start -D /dev/tty-3 -list_devices -list_files -list_tasks -list_topics diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config deleted file mode 100644 index 0a83b68fae..0000000000 --- a/posix-configs/eagle/210qc/px4-flight.config +++ /dev/null @@ -1,97 +0,0 @@ -uorb start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -param set ATT_W_MAG 0.00 -param set PE_MAG_NOISE 1.0f -param set SENS_BOARD_ROT 0 -param set MPU_GYRO_LPF_ENM 4 -param set MPU_ACC_LPF_ENM 4 -param set MPU_SAMPLE_R_ENM 2 -param set GYRO0_XOFF -0.0028 -param set GYRO0_YOFF -0.0047 -param set GYRO0_ZOFF -0.0034 -param set GYRO0_XSCALE 1.0000 -param set GYRO0_YSCALE 1.0000 -param set GYRO0_ZSCALE 1.0000 -param set MAG0_XOFF 0.0000 -param set MAG0_YOFF 0.0000 -param set MAG0_ZOFF 0.0000 -param set MAG0_XSCALE 1.0000 -param set MAG0_YSCALE 1.0000 -param set MAG0_ZSCALE 1.0000 -param set ACC0_XOFF -0.0941 -param set ACC0_YOFF 0.1168 -param set ACC0_ZOFF -0.0398 -param set ACC0_XSCALE 1.0004 -param set ACC0_YSCALE 0.9974 -param set ACC0_ZSCALE 0.9951 -param set RC_RECEIVER_TYPE 1 -param set UART_ESC_MODEL 2 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 4 -param set UART_ESC_MOTOR2 2 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 -sleep 1 -commander start -rc_receiver start -D /dev/tty-1 -sleep 1 -mpu9x50 start -D /dev/spi-1 -df_bmp280_wrapper start -sensors start -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -land_detector start multicopter -sleep 1 -uart_esc start -D /dev/tty-2 -list_devices -list_files -list_tasks -list_topics diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4ce21a6dbd..6384d415aa 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -175,7 +175,7 @@ typedef struct { int do_accel_calibration(orb_advert_t *mavlink_log_pub) { -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) int fd; #endif @@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); @@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) return ERROR; } -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = px4_open(str, 0); @@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub break; } -#ifdef __PX4_QURT +#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct accel_report accel_report; orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2f93432bc9..a9b005c0f7 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -186,7 +186,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); if (fd >= 0) { @@ -239,9 +239,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) for (unsigned s = 0; s < gyro_count; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); -#ifdef __PX4_QURT +#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) // For QURT respectively the driver framework, we need to get the device ID by copying one report. - struct gyro_report gyro_report; + struct gyro_report gyro_report; orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report); worker_data.device_id[s] = gyro_report.device_id; #endif @@ -337,7 +337,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_GYRO%u_ID", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); -#ifndef __PX4_QURT +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cbc3e062ac..116911bb3a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -46,9 +46,6 @@ * @author Anton Babushkin */ -// TODO-JYW: TESTING-TESTING -#define DEBUG_BUILD 1 - #include #include diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 0f4fe127c0..e31ba33d02 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -77,7 +78,7 @@ using namespace DriverFramework; class DfMpu9250Wrapper : public MPU9250 { public: - DfMpu9250Wrapper(/*enum Rotation rotation*/); + DfMpu9250Wrapper(bool mag_enabled); ~DfMpu9250Wrapper(); @@ -105,11 +106,13 @@ private: void _update_accel_calibration(); void _update_gyro_calibration(); + void _update_mag_calibration(); //enum Rotation _rotation; orb_advert_t _accel_topic; orb_advert_t _gyro_topic; + orb_advert_t _mag_topic; orb_advert_t _mavlink_log_pub; @@ -133,11 +136,22 @@ private: float z_scale; } _gyro_calibration; + struct mag_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _mag_calibration; + int _accel_orb_class_instance; int _gyro_orb_class_instance; + int _mag_orb_class_instance; Integrator _accel_int; Integrator _gyro_int; + Integrator _mag_int; unsigned _publish_count; @@ -147,24 +161,31 @@ private: perf_counter_t _fifo_corruption_counter; perf_counter_t _gyro_range_hit_counter; perf_counter_t _accel_range_hit_counter; + perf_counter_t _mag_range_hit_counter; perf_counter_t _publish_perf; hrt_abstime _last_accel_range_hit_time; uint64_t _last_accel_range_hit_count; + + bool _mag_enabled; }; -DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : - MPU9250(IMU_DEVICE_PATH), +DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) : + MPU9250(IMU_DEVICE_PATH, mag_enabled), _accel_topic(nullptr), _gyro_topic(nullptr), + _mag_topic(nullptr), _mavlink_log_pub(nullptr), _param_update_sub(-1), _accel_calibration{}, _gyro_calibration{}, + _mag_calibration{}, _accel_orb_class_instance(-1), _gyro_orb_class_instance(-1), + _mag_orb_class_instance(-1), _accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false), _gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true), + _mag_int(MPU9250_NEVER_AUTOPUBLISH_US, true), /*_rotation(rotation)*/ _publish_count(0), _read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")), @@ -173,9 +194,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")), _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")), _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")), + _mag_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_mag_range_hits")), _publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")), _last_accel_range_hit_time(0), - _last_accel_range_hit_count(0) + _last_accel_range_hit_count(0), + _mag_enabled(mag_enabled) { // Set sane default calibration values _accel_calibration.x_scale = 1.0f; @@ -191,6 +214,15 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : _gyro_calibration.x_offset = 0.0f; _gyro_calibration.y_offset = 0.0f; _gyro_calibration.z_offset = 0.0f; + + if (_mag_enabled == true) { + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; + } } DfMpu9250Wrapper::~DfMpu9250Wrapper() @@ -201,6 +233,9 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper() perf_free(_fifo_corruption_counter); perf_free(_gyro_range_hit_counter); perf_free(_accel_range_hit_counter); + if (_mag_enabled == true) { + perf_free(_mag_range_hit_counter); + } perf_free(_publish_perf); } @@ -226,6 +261,18 @@ int DfMpu9250Wrapper::start() return -1; } + if (_mag_enabled == true) { + // TODO: don't publish garbage here + mag_report mag_report = {}; + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, + &_mag_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_mag_topic == nullptr) { + PX4_ERR("sensor_mag advert fail"); + return -1; + } + } + /* Subscribe to param update topic. */ if (_param_update_sub < 0) { _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -251,6 +298,7 @@ int DfMpu9250Wrapper::start() /* Force getting the calibration values. */ _update_accel_calibration(); _update_gyro_calibration(); + _update_mag_calibration(); return 0; } @@ -276,6 +324,9 @@ void DfMpu9250Wrapper::info() perf_print_counter(_fifo_corruption_counter); perf_print_counter(_gyro_range_hit_counter); perf_print_counter(_accel_range_hit_counter); + if (_mag_enabled == true) { + perf_print_counter(_mag_range_hit_counter); + } perf_print_counter(_publish_perf); } @@ -430,6 +481,83 @@ void DfMpu9250Wrapper::_update_accel_calibration() _accel_calibration.z_offset = 0.0f; } +void DfMpu9250Wrapper::_update_mag_calibration() +{ + if (_mag_enabled == false) { + return; + } + + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { + + // TODO: remove printfs and add error counter + + char str[30]; + (void)sprintf(str, "CAL_MAG%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + res = param_get(param_find(str), &_mag_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + res = param_get(param_find(str), &_mag_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + res = param_get(param_find(str), &_mag_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + res = param_get(param_find(str), &_mag_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + res = param_get(param_find(str), &_mag_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + res = param_get(param_find(str), &_mag_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + } + + // Set sane default calibration values + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; +} + int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) { /* Check if calibration values are still up-to-date. */ @@ -442,6 +570,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) _update_accel_calibration(); _update_gyro_calibration(); + _update_mag_calibration(); } math::Vector<3> vec_integrated_unused; @@ -491,13 +620,20 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); + if (_mag_enabled == true) { + perf_set_count(_mag_range_hit_counter, data.mag_range_hit_counter); + } perf_begin(_publish_perf); accel_report accel_report = {}; gyro_report gyro_report = {}; + mag_report mag_report = {}; accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + if (_mag_enabled == true) { + mag_report.timestamp = accel_report.timestamp; + } // TODO: get these right gyro_report.scaling = -1.0f; @@ -508,6 +644,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.range_m_s2 = -1.0f; accel_report.device_id = m_id.dev_id; + if (_mag_enabled == true) { + mag_report.scaling = -1.0f; + mag_report.range_ga = -1.0f; + mag_report.device_id = m_id.dev_id; + } + // TODO: remove these (or get the values) gyro_report.x_raw = NAN; gyro_report.y_raw = NAN; @@ -517,6 +659,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.y_raw = NAN; accel_report.z_raw = NAN; + if (_mag_enabled == true) { + mag_report.x_raw = NAN; + mag_report.y_raw = NAN; + mag_report.z_raw = NAN; + } + math::Vector<3> gyro_val_filt; math::Vector<3> accel_val_filt; @@ -533,6 +681,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.y = accel_val_filt(1); accel_report.z = accel_val_filt(2); + if (_mag_enabled == true) { + mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale; + mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale; + mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale; + } + gyro_report.x_integral = gyro_val_integ(0); gyro_report.y_integral = gyro_val_integ(1); gyro_report.z_integral = gyro_val_integ(2); @@ -552,6 +706,10 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } + if ((_mag_topic != nullptr) && (_mag_enabled == true)) { + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + } + /* Notify anyone waiting for data. */ DevMgr::updateNotify(*this); @@ -588,9 +746,9 @@ int stop(); int info(); void usage(); -int start(/*enum Rotation rotation*/) +int start(bool mag_enabled) { - g_dev = new DfMpu9250Wrapper(/*rotation*/); + g_dev = new DfMpu9250Wrapper(mag_enabled); if (g_dev == nullptr) { PX4_ERR("failed instantiating DfMpu9250Wrapper object"); @@ -695,9 +853,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[]) const char *verb = argv[myoptind]; + if (!strcmp(verb, "start_with_mag")) { + ret = df_mpu9250_wrapper::start(true); + } - if (!strcmp(verb, "start")) { - ret = df_mpu9250_wrapper::start(/*rotation*/); + else if (!strcmp(verb, "start")) { + ret = df_mpu9250_wrapper::start(false); } else if (!strcmp(verb, "stop")) {