fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added mag support to the DriverFramework mpu9250 driver. Shortened parameter names for legacy drivers. Added temporary ifdef's in the calibration code for Snapdragon Flight builds.

Browse Source 
Signed-off-by: jwilson <jwilson@qti.qualcomm.com>
This commit is contained in:
jwilson 2016-05-26 21:21:37 -07:00 committed by Lorenz Meier
parent d9422e0296
commit c6250657eb
13 changed files with 202 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cmake/configs/posix_eagle_legacy_driver_default.cmake
View File

@ -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

8
cmake/configs/qurt_eagle_legacy_driver_default.cmake
View File

@ -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
)

6
posix-configs/eagle/200qx/px4-calib.config
View File

@ -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

16
posix-configs/eagle/200qx/px4-flight-v1-board.config
View File

@ -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

16
posix-configs/eagle/200qx/px4-flight-v2-board.config
View File

@ -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

8
posix-configs/eagle/210qc/mainapp-calib.config
View File

@ -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

7
posix-configs/eagle/210qc/mainapp-flight.config
View File

@ -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

72
posix-configs/eagle/210qc/px4-calib.config
View File

@ -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

97
posix-configs/eagle/210qc/px4-flight.config
View File

@ -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

8
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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);

8
src/modules/commander/gyro_calibration.cpp
View File

@ -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);

3
src/modules/sensors/sensors.cpp
View File

@ -46,9 +46,6 @@
* @author Anton Babushkin <anton@px4.io>
*/
// TODO-JYW: TESTING-TESTING
#define DEBUG_BUILD 1
#include <board_config.h>
#include <px4_config.h>

177
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
View File

@ -58,6 +58,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/device/integrator.h>
#include <uORB/topics/parameter_update.h>
@ -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")) {