dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: move mag aggregation to new VehicleMagnetometer WorkItem 

- purge all reminaing magnetometer IOCTL usage
 - mag calibration add off diagonal (soft iron) scale factors
This commit is contained in:
Daniel Agar 2020-08-16 21:56:15 -04:00
parent ad148af2fd
commit 971b1e4b4d
72 changed files with 870 additions and 1134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@ -866,6 +866,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_local_position"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_magnetometer"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
@ -932,6 +933,7 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_local_position"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_magnetometer"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"'

10
Tools/ecl_ekf/plotting/pdf_report.py
View File

@ -46,10 +46,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
raise PreconditionError('could not find innovation data')
try:
sensor_preflight = ulog.get_dataset('sensor_preflight').data
sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data
print('found sensor_preflight data')
except:
raise PreconditionError('could not find sensor_preflight data')
raise PreconditionError('could not find sensor_preflight_imu data')
control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
@ -61,10 +61,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
with PdfPages(output_plot_filename) as pdf_pages:
# plot IMU consistency data
if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and (
'gyro_inconsistency_rad_s' in sensor_preflight.keys()):
if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and (
'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()):
data_plot = TimeSeriesPlot(
sensor_preflight, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
x_labels=['data index', 'data index'],
y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages)

3
msg/CMakeLists.txt
View File

@ -119,7 +119,8 @@ set(msg_files
sensor_gyro.msg
sensor_gyro_fifo.msg
sensor_mag.msg
sensor_preflight.msg
sensor_preflight_imu.msg
sensor_preflight_mag.msg
sensor_selection.msg
subsystem_info.msg
system_power.msg

2
msg/sensor_mag.msg
View File

@ -11,4 +11,4 @@ float32 temperature # temperature in degrees celsius
uint32 error_count
bool is_external # if true the mag is external (i.e. not built into the board)
bool is_external # if true the mag is external (i.e. not built into the board)

1
msg/sensor_preflight.msg → msg/sensor_preflight_imu.msg
View File

@ -5,4 +5,3 @@
uint64 timestamp # time since system start (microseconds)
float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s).
float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians.

7
msg/sensor_preflight_mag.msg Normal file
View File

@ -0,0 +1,7 @@
#
# Pre-flight sensor check metrics.
# The topic will not be updated when the vehicle is armed
#
uint64 timestamp # time since system start (microseconds)
float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians.

1
msg/sensor_selection.msg
View File

@ -5,4 +5,3 @@
uint64 timestamp # time since system start (microseconds)
uint32 accel_device_id # unique device ID for the selected accelerometers
uint32 gyro_device_id # unique device ID for the selected rate gyros
uint32 mag_device_id # unique device ID for the selected magnetometer

4
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -158,7 +158,7 @@ rtps:
id: 67
- msg: sensor_mag
id: 68
- msg: sensor_preflight
- msg: sensor_preflight_imu
id: 69
- msg: sensor_selection
id: 70
@ -297,6 +297,8 @@ rtps:
id: 132
- msg: telemetry_heartbeat
id: 133
- msg: sensor_preflight_mag
id: 134
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

9
msg/vehicle_magnetometer.msg
View File

@ -1,3 +1,8 @@
uint64 timestamp # time since system start (microseconds)
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
uint32 device_id # unique device ID for the selected magnetometer
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss

26
src/drivers/drv_mag.h
View File

@ -51,30 +51,4 @@
#include <uORB/topics/sensor_mag.h>
/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
struct mag_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
* ioctl() definitions
*/
#define _MAGIOCBASE (0x2400)
#define _MAGIOC(_n) (_PX4_IOC(_MAGIOCBASE, _n))
/** set the mag scaling constants to the structure pointed to by (arg) */
#define MAGIOCSSCALE _MAGIOC(4)
/** copy the mag scaling constants to the structure pointed to by (arg) */
#define MAGIOCGSCALE _MAGIOC(5)
/** determine if mag is external or onboard */
#define MAGIOCGEXTERNAL _MAGIOC(11)
#endif /* _DRV_MAG_H */

2
src/drivers/imu/adis16448/ADIS16448.cpp
View File

@ -307,8 +307,6 @@ ADIS16448::print_status()
perf_print_counter(_perf_transfer);
perf_print_counter(_perf_bad_transfer);
perf_print_counter(_perf_crc_bad);
_px4_mag.print_status();
}
void

2
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
View File

@ -75,8 +75,6 @@ void ICM20948_AK09916::PrintInfo()
{
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
void ICM20948_AK09916::Run()

2
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp
View File

@ -75,8 +75,6 @@ void MPU9250_AK8963::PrintInfo()
{
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
void MPU9250_AK8963::Run()

2
src/drivers/imu/mpu9250/MPU9250_mag.h
View File

@ -124,8 +124,6 @@ public:
bool ak8963_check_id(uint8_t &id);
bool ak8963_read_adjustments();
void print_status() { _px4_mag.print_status(); }
protected:
device::Device *_interface;

2
src/drivers/imu/mpu9250/mpu9250.cpp
View File

@ -669,6 +669,4 @@ MPU9250::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_mag.print_status();
}

2
src/drivers/magnetometer/akm/ak09916/AK09916.cpp
View File

@ -84,8 +84,6 @@ void AK09916::print_status()
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
int AK09916::probe()

2
src/drivers/magnetometer/akm/ak8963/AK8963.cpp
View File

@ -84,8 +84,6 @@ void AK8963::print_status()
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
int AK8963::probe()

1
src/drivers/magnetometer/bmm150/bmm150.cpp
View File

@ -549,7 +549,6 @@ void BMM150::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_good_transfers);
_px4_mag.print_status();
}
int BMM150::self_test()

1
src/drivers/magnetometer/hmc5883/HMC5883.cpp
View File

@ -482,5 +482,4 @@ void HMC5883::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("interval: %u us\n", _measure_interval);
_px4_mag.print_status();
}

1
src/drivers/magnetometer/hmc5883/HMC5883.hpp
View File

@ -39,7 +39,6 @@
#include <drivers/device/i2c.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

1
src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp
View File

@ -39,7 +39,6 @@
#include <px4_platform_common/px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "hmc5883.h"

1
src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp
View File

@ -39,7 +39,6 @@
#include <px4_platform_common/px4_config.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "hmc5883.h"

1
src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
View File

@ -81,7 +81,6 @@ void IST8308::print_status()
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
_px4_mag.print_status();
}
int IST8308::probe()

2
src/drivers/magnetometer/ist8310/ist8310.cpp
View File

@ -49,7 +49,6 @@
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
@ -581,7 +580,6 @@ void IST8310::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u interval\n", _measure_interval);
_px4_mag.print_status();
}
I2CSPIDriverBase *

1
src/drivers/magnetometer/lis2mdl/lis2mdl.cpp
View File

@ -163,7 +163,6 @@ LIS2MDL::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u", _measure_interval);
_px4_mag.print_status();
}
void

1
src/drivers/magnetometer/lis2mdl/lis2mdl.h
View File

@ -41,7 +41,6 @@
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>

1
src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/drivers/magnetometer/lis3mdl/lis3mdl.cpp
View File

@ -195,7 +195,6 @@ void LIS3MDL::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u", _measure_interval);
_px4_mag.print_status();
}
int LIS3MDL::reset()

1
src/drivers/magnetometer/lis3mdl/lis3mdl.h
View File

@ -41,7 +41,6 @@
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>

1
src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp
View File

@ -334,5 +334,4 @@ void LSM303AGR::print_status()
perf_print_counter(_mag_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_bad_values);
_px4_mag.print_status();
}

1
src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp
View File

@ -35,7 +35,6 @@
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

1
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp
View File

@ -200,5 +200,4 @@ void LSM9DS1_MAG::print_status()
perf_print_counter(_interval_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_data_overrun_perf);
_px4_mag.print_status();
}

1
src/drivers/magnetometer/qmc5883/QMC5883.cpp
View File

@ -308,5 +308,4 @@ void QMC5883::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("interval: %u us\n", _measure_interval);
_px4_mag.print_status();
}

1
src/drivers/magnetometer/qmc5883/QMC5883.hpp
View File

@ -45,7 +45,6 @@
#include <drivers/device/i2c.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_device.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

1
src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp
View File

@ -39,7 +39,6 @@
#include <px4_platform_common/px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "qmc5883.h"

1
src/drivers/magnetometer/rm3100/rm3100.cpp
View File

@ -267,7 +267,6 @@ void RM3100::print_status()
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u", _measure_interval);
_px4_mag.print_status();
}
int RM3100::reset()

1
src/drivers/magnetometer/rm3100/rm3100.h
View File

@ -41,7 +41,6 @@
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/i2c_spi_buses.h>

1
src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/drivers/magnetometer/rm3100/rm3100_spi.cpp
View File

@ -49,7 +49,6 @@
#include <unistd.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"

1
src/lib/CMakeLists.txt
View File

@ -51,7 +51,6 @@ add_subdirectory(hysteresis)
add_subdirectory(l1)
add_subdirectory(landing_slope)
add_subdirectory(led)
add_subdirectory(mag_compensation)
add_subdirectory(mathlib)
add_subdirectory(mixer)
add_subdirectory(mixer_module)

79
src/lib/drivers/magnetometer/PX4Magnetometer.cpp
View File

@ -38,13 +38,14 @@
PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
CDev(nullptr),
_sensor_mag_pub{ORB_ID(sensor_mag), priority},
_rotation{rotation},
_device_id{device_id}
_sensor_pub{ORB_ID(sensor_mag), priority},
_device_id{device_id},
_rotation{rotation}
{
_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
// advertise immediately to keep instance numbering in sync
_sensor_pub.advertise();
_sensor_mag_pub.advertise();
_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
}
PX4Magnetometer::~PX4Magnetometer()
@ -53,46 +54,7 @@ PX4Magnetometer::~PX4Magnetometer()
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance);
}
_sensor_mag_pub.unadvertise();
}
int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case MAGIOCSSCALE: {
// Copy offsets and scale factors in
mag_calibration_s cal{};
memcpy(&cal, (mag_calibration_s *) arg, sizeof(cal));
_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
_calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
}
return PX4_OK;
case MAGIOCGSCALE: {
// copy out scale factors
mag_calibration_s cal{};
cal.x_offset = _calibration_offset(0);
cal.y_offset = _calibration_offset(1);
cal.z_offset = _calibration_offset(2);
cal.x_scale = _calibration_scale(0);
cal.y_scale = _calibration_scale(1);
cal.z_scale = _calibration_scale(2);
memcpy((mag_calibration_s *)arg, &cal, sizeof(cal));
}
return 0;
case MAGIOCGEXTERNAL:
return _external;
case DEVIOCGDEVICEID:
return _device_id;
default:
return -ENOTTY;
}
_sensor_pub.unadvertise();
}
void PX4Magnetometer::set_device_type(uint8_t devtype)
@ -104,11 +66,11 @@ void PX4Magnetometer::set_device_type(uint8_t devtype)
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
// copy back
_device_id = device_id.devid;
}
void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
void PX4Magnetometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
{
sensor_mag_s report;
report.timestamp_sample = timestamp_sample;
@ -119,27 +81,12 @@ void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, flo
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw_f{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((raw_f * _scale) - _calibration_offset).emult(_calibration_scale))};
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.x = x * _scale;
report.y = y * _scale;
report.z = z * _scale;
report.is_external = _external;
report.timestamp = hrt_absolute_time();
_sensor_mag_pub.publish(report);
}
void PX4Magnetometer::print_status()
{
PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
(double)_calibration_scale(2));
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));
_sensor_pub.publish(report);
}

23
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
View File

@ -37,7 +37,6 @@
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_mag.h>
@ -47,10 +46,9 @@ public:
PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
~PX4Magnetometer() override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
bool external() { return _external; }
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint32_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
@ -58,24 +56,19 @@ public:
void set_temperature(float temperature) { _temperature = temperature; }
void set_external(bool external) { _external = external; }
void update(hrt_abstime timestamp_sample, float x, float y, float z);
void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
int get_class_instance() { return _class_device_instance; };
void print_status();
private:
uORB::PublicationMulti<sensor_mag_s> _sensor_mag_pub;
uORB::PublicationMulti<sensor_mag_s> _sensor_pub;
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
uint32_t _device_id{0};
const enum Rotation _rotation;
const enum Rotation _rotation;
uint32_t _device_id{0};
float _temperature{NAN};
float _scale{1.f};
uint32_t _error_count{0};
float _scale{1.f};
float _temperature{NAN};
uint32_t _error_count{0};
bool _external{false};

53
src/lib/mag_compensation/MagCompensation.cpp
View File

@ -1,53 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file MagCompensation.cpp
*
*/
#include "MagCompensation.hpp"
#include <mathlib/mathlib.h>
MagCompensator::MagCompensator(ModuleParams *parent):
ModuleParams(parent)
{
}
void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect)
{
if (_armed) {
mag = mag + param_vect * _power;
}
}

63
src/lib/mag_compensation/MagCompensation.hpp
View File

@ -1,63 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file MagCompensation.hpp
* @author Roman Bapst <roman@auterion.com>
*
* Library for magnetometer data compensation.
*
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
class MagCompensator : public ModuleParams
{
public:
MagCompensator(ModuleParams *parent);
~MagCompensator() = default;
void update_armed_flag(bool armed) { _armed = armed; }
void update_power(float power) { _power = power; }
void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect);
private:
float _power{0};
bool _armed{false};
};

44
src/lib/mag_compensation/mag_compensation_params.c
View File

@ -1,44 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* Type of magnetometer compensation
*
* @value 0 Disabled
* @value 1 Throttle-based compensation
* @value 2 Current-based compensation (battery_status instance 0)
* @value 3 Current-based compensation (battery_status instance 1)
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);

68
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -87,11 +87,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_mag == 1) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
@ -99,29 +94,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
failed = true;
}
// TODO: highest priority mag
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
@ -132,10 +112,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- ACCEL ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
@ -143,63 +119,31 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
int32_t device_id = -1;
if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- BARO ---- */

6
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
View File

@ -38,7 +38,7 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@ -47,9 +47,9 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_imu_s &sensors = sensors_sub.get();
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.

6
src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
View File

@ -40,7 +40,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_mag.h>
// return false if the magnetomer measurements are inconsistent
bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@ -49,9 +49,9 @@ bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
bool pass = false; // flag for result of checks
// get the sensor preflight data
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_mag_s &sensors = sensors_sub.get();
if (sensors.timestamp == 0) {
// can happen if not advertised (yet)

12
src/modules/ekf2/ekf2_main.cpp
View File

@ -886,8 +886,8 @@ void Ekf2::Run()
// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
// and notification events
// Check if there has been a persistant change in magnetometer ID
if (_sensor_selection.mag_device_id != 0
&& (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
if (magnetometer.device_id != 0
&& (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
if (_invalid_mag_id_count < 200) {
_invalid_mag_id_count++;
@ -908,7 +908,7 @@ void Ekf2::Run()
_param_ekf2_magbias_y.commit_no_notification();
_param_ekf2_magbias_z.set(0.f);
_param_ekf2_magbias_z.commit_no_notification();
_param_ekf2_magbias_id.set(_sensor_selection.mag_device_id);
_param_ekf2_magbias_id.set(magnetometer.device_id);
_param_ekf2_magbias_id.commit();
_invalid_mag_id_count = 0;
@ -1510,7 +1510,7 @@ void Ekf2::Run()
bias.accel_device_id = _sensor_selection.accel_device_id;
}
bias.mag_device_id = _sensor_selection.mag_device_id;
bias.mag_device_id = _param_ekf2_magbias_id.get();
// In-run bias estimates
_ekf.getGyroBias().copyTo(bias.gyro_bias);
@ -1632,8 +1632,7 @@ void Ekf2::Run()
// Check and save the last valid calibration when we are disarmed
if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& (status.filter_fault_flags == 0)
&& (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) {
&& (status.filter_fault_flags == 0)) {
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
@ -1642,7 +1641,6 @@ void Ekf2::Run()
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
}
}
publish_wind_estimate(now);

3
src/modules/logger/logged_topics.cpp
View File

@ -79,7 +79,8 @@ void LoggedTopics::add_default_topics()
add_topic("safety");
add_topic("sensor_combined");
add_topic("sensor_correction");
add_topic("sensor_preflight", 200);
add_topic("sensor_preflight_imu", 200);
add_topic("sensor_preflight_mag", 500);
add_topic("sensor_selection");
add_topic("system_power", 500);
add_topic("tecs_status", 200);

4
src/modules/sensors/CMakeLists.txt
View File

@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_air_data)
add_subdirectory(vehicle_imu)
add_subdirectory(vehicle_magnetometer)
px4_add_module(
MODULE modules__sensors
@ -45,7 +46,6 @@ px4_add_module(
SRCS
voted_sensors_update.cpp
sensors.cpp
parameters.cpp
DEPENDS
airspeed
conversion
@ -58,5 +58,5 @@ px4_add_module(
vehicle_angular_velocity
vehicle_air_data
vehicle_imu
mag_compensation
vehicle_magnetometer
)

52
src/modules/sensors/common.h
View File

@ -1,52 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file common.h
* common definitions used in sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
namespace sensors
{
constexpr uint8_t MAG_COUNT_MAX = 4;
constexpr uint8_t GYRO_COUNT_MAX = 3;
constexpr uint8_t ACCEL_COUNT_MAX = 3;
constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX));
} /* namespace sensors */

2
src/modules/sensors/data_validator/tests/tests_common.cpp
View File

@ -100,4 +100,4 @@ void fill_validator_with_samples(DataValidator *validator,
*timestamp_io = timestamp;
*value_io = val;
}
}

147
src/modules/sensors/parameters.cpp
View File

@ -1,147 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file parameters.cpp
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
namespace sensors
{
void initialize_parameter_handles(ParameterHandles &parameter_handles)
{
/* Differential pressure offset */
parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
/* rotations */
parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* mag compensation */
parameter_handles.mag_comp_type = param_find("CAL_MAG_COMP_TYP");
parameter_handles.mag_comp_paramX[0] = param_find("CAL_MAG0_XCOMP");
parameter_handles.mag_comp_paramX[1] = param_find("CAL_MAG1_XCOMP");
parameter_handles.mag_comp_paramX[2] = param_find("CAL_MAG2_XCOMP");
parameter_handles.mag_comp_paramX[3] = param_find("CAL_MAG3_XCOMP");
parameter_handles.mag_comp_paramY[0] = param_find("CAL_MAG0_YCOMP");
parameter_handles.mag_comp_paramY[1] = param_find("CAL_MAG1_YCOMP");
parameter_handles.mag_comp_paramY[2] = param_find("CAL_MAG2_YCOMP");
parameter_handles.mag_comp_paramY[3] = param_find("CAL_MAG3_YCOMP");
parameter_handles.mag_comp_paramZ[0] = param_find("CAL_MAG0_ZCOMP");
parameter_handles.mag_comp_paramZ[1] = param_find("CAL_MAG1_ZCOMP");
parameter_handles.mag_comp_paramZ[2] = param_find("CAL_MAG2_ZCOMP");
parameter_handles.mag_comp_paramZ[3] = param_find("CAL_MAG3_ZCOMP");
parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
(void)param_find("BAT_V_DIV");
(void)param_find("BAT_A_PER_V");
(void)param_find("CAL_ACC0_ID");
(void)param_find("CAL_GYRO0_ID");
(void)param_find("CAL_MAG0_ID");
(void)param_find("CAL_MAG1_ID");
(void)param_find("CAL_MAG2_ID");
(void)param_find("CAL_MAG3_ID");
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("CAL_MAG3_ROT");
(void)param_find("CAL_MAG_SIDES");
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");
(void)param_find("TRIG_MODE");
(void)param_find("UAVCAN_ENABLE");
// Parameters controlling the on-board sensor thermal calibrator
(void)param_find("SYS_CAL_TDEL");
(void)param_find("SYS_CAL_TMAX");
(void)param_find("SYS_CAL_TMIN");
}
void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
/* Airspeed offset */
param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa));
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_get(parameter_handles.board_rotation, &(parameters.board_rotation));
param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0]));
param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
param_get(parameter_handles.mag_comp_type, &(parameters.mag_comp_type));
param_get(parameter_handles.mag_comp_paramX[0], &(parameters.mag_comp_paramX[0]));
param_get(parameter_handles.mag_comp_paramX[1], &(parameters.mag_comp_paramX[1]));
param_get(parameter_handles.mag_comp_paramX[2], &(parameters.mag_comp_paramX[2]));
param_get(parameter_handles.mag_comp_paramX[3], &(parameters.mag_comp_paramX[3]));
param_get(parameter_handles.mag_comp_paramY[0], &(parameters.mag_comp_paramY[0]));
param_get(parameter_handles.mag_comp_paramY[1], &(parameters.mag_comp_paramY[1]));
param_get(parameter_handles.mag_comp_paramY[2], &(parameters.mag_comp_paramY[2]));
param_get(parameter_handles.mag_comp_paramY[3], &(parameters.mag_comp_paramY[3]));
param_get(parameter_handles.mag_comp_paramZ[0], &(parameters.mag_comp_paramZ[0]));
param_get(parameter_handles.mag_comp_paramZ[1], &(parameters.mag_comp_paramZ[1]));
param_get(parameter_handles.mag_comp_paramZ[2], &(parameters.mag_comp_paramZ[2]));
param_get(parameter_handles.mag_comp_paramZ[3], &(parameters.mag_comp_paramZ[3]));
param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
}
} /* namespace sensors */

103
src/modules/sensors/parameters.h
View File

@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file parameters.h
*
* defines the list of parameters that are used within the sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include <lib/parameters/param.h>
namespace sensors
{
struct Parameters {
float diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
int32_t board_rotation;
float board_offset[3];
//parameters for current/throttle-based mag compensation
int32_t mag_comp_type;
float mag_comp_paramX[4];
float mag_comp_paramY[4];
float mag_comp_paramZ[4];
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
};
struct ParameterHandles {
param_t diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_t board_rotation;
param_t board_offset[3];
param_t mag_comp_type;
param_t mag_comp_paramX[4];
param_t mag_comp_paramY[4];
param_t mag_comp_paramZ[4];
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;
};
/**
* initialize ParameterHandles struct
*/
void initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on success, <0 on error
*/
void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
} /* namespace sensors */

29
src/modules/sensors/sensor_params_mag.c
View File

@ -62,3 +62,32 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
* @group Sensors
*/
PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
/**
* Type of magnetometer compensation
*
* @value 0 Disabled
* @value 1 Throttle-based compensation
* @value 2 Current-based compensation (battery_status instance 0)
* @value 3 Current-based compensation (battery_status instance 1)
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);
/**
* Magnetometer max rate.
*
* Magnetometer data maximum publication rate. This is an upper bound,
* actual magnetometer data rate is still dependant on the sensor.
*
* @min 1
* @max 200
* @group Sensors
* @unit Hz
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);

26
src/modules/sensors/sensor_params_mag0.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis

26
src/modules/sensors/sensor_params_mag1.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis

26
src/modules/sensors/sensor_params_mag2.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis

26
src/modules/sensors/sensor_params_mag3.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis

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

@ -45,7 +45,6 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/airspeed/airspeed.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -65,19 +64,17 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/battery_status.h>
#include "parameters.h"
#include "voted_sensors_update.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
using namespace sensors;
using namespace time_literals;
@ -116,10 +113,9 @@ private:
hrt_abstime _last_config_update{0};
hrt_abstime _sensor_combined_prev_timestamp{0};
hrt_abstime _magnetometer_prev_timestamp{0};
sensor_combined_s _sensor_combined{};
sensor_preflight_s _sensor_preflight{};
sensor_preflight_imu_s _sensor_preflight_imu{};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
{this, ORB_ID(vehicle_imu), 0},
@ -127,26 +123,14 @@ private:
{this, ORB_ID(vehicle_imu), 2}
};
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; /**< battery_status instance 0 subscription */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
uORB::Publication<sensor_preflight_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
enum class MagCompensationType {
Disabled = 0,
Throttle,
Current_inst0,
Current_inst1
};
MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
uORB::Publication<sensor_preflight_imu_s> _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)};
perf_counter_t _loop_perf; /**< loop performance counter */
@ -161,14 +145,35 @@ private:
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
Parameters _parameters{}; /**< local copies of interesting parameters */
ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */
struct Parameters {
float diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
} _parameters{}; /**< local copies of interesting parameters */
struct ParameterHandles {
param_t diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;
} _parameter_handles{}; /**< handles for interesting parameters */
VotedSensorsUpdate _voted_sensors_update;
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData *_vehicle_air_data{nullptr};
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@ -202,9 +207,11 @@ private:
void InitializeVehicleAirData();
void InitializeVehicleIMU();
void InitializeVehicleMagnetometer();
DEFINE_PARAMETERS(
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
)
};
@ -213,9 +220,39 @@ Sensors::Sensors(bool hil_enabled) :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_hil_enabled(hil_enabled),
_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
_voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub)
_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
{
initialize_parameter_handles(_parameter_handles);
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
param_find("BAT_V_DIV");
param_find("BAT_A_PER_V");
param_find("CAL_ACC0_ID");
param_find("CAL_GYRO0_ID");
param_find("SENS_BOARD_ROT");
param_find("SENS_BOARD_X_OFF");
param_find("SENS_BOARD_Y_OFF");
param_find("SENS_BOARD_Z_OFF");
param_find("SYS_PARAM_VER");
param_find("SYS_AUTOSTART");
param_find("SYS_AUTOCONFIG");
param_find("TRIG_MODE");
param_find("UAVCAN_ENABLE");
// Parameters controlling the on-board sensor thermal calibrator
param_find("SYS_CAL_TDEL");
param_find("SYS_CAL_TMAX");
param_find("SYS_CAL_TMIN");
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
@ -239,6 +276,11 @@ Sensors::~Sensors()
delete _vehicle_air_data;
}
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Stop();
delete _vehicle_magnetometer;
}
for (auto &vehicle_imu : _vehicle_imu_list) {
if (vehicle_imu) {
vehicle_imu->Stop();
@ -265,8 +307,15 @@ int Sensors::parameters_update()
return 0;
}
/* read the parameter values into _parameters */
update_parameters(_parameter_handles, _parameters);
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
_voted_sensors_update.parametersUpdate();
@ -474,6 +523,21 @@ void Sensors::InitializeVehicleIMU()
}
}
void Sensors::InitializeVehicleMagnetometer()
{
if (_param_sys_has_mag.get()) {
if (_vehicle_magnetometer == nullptr) {
if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
_vehicle_magnetometer = new VehicleMagnetometer();
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Start();
}
}
}
}
}
void Sensors::Run()
{
if (should_exit()) {
@ -490,6 +554,7 @@ void Sensors::Run()
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleMagnetometer();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
}
@ -505,57 +570,16 @@ void Sensors::Run()
if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
_armed = vcontrol_mode.flag_armed;
_voted_sensors_update.update_mag_comp_armed(_armed);
}
//check mag power compensation type (change battery current subscription instance if necessary)
if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0
&& _mag_comp_type != MagCompensationType::Current_inst0) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
}
if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1
&& _mag_comp_type != MagCompensationType::Current_inst1) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
}
_mag_comp_type = (MagCompensationType)_parameters.mag_comp_type;
//update power signal for mag compensation
if (_mag_comp_type == MagCompensationType::Throttle) {
actuator_controls_s controls {};
if (_actuator_ctrl_0_sub.update(&controls)) {
_voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
} else if (_mag_comp_type == MagCompensationType::Current_inst0
|| _mag_comp_type == MagCompensationType::Current_inst1) {
battery_status_s bat_stat {};
if (_battery_status_sub.update(&bat_stat)) {
_voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA]
}
}
}
vehicle_magnetometer_s magnetometer{};
_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
_voted_sensors_update.sensorsPoll(_sensor_combined);
// check analog airspeed
adc_poll();
diff_pres_poll();
if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
_magnetometer_pub.publish(magnetometer);
_magnetometer_prev_timestamp = magnetometer.timestamp;
if (!_armed) {
_voted_sensors_update.calcMagInconsistency(_sensor_preflight);
}
}
if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
@ -565,11 +589,11 @@ void Sensors::Run()
// If the the vehicle is disarmed calculate the length of the maximum difference between
// IMU units as a consistency metric and publish to the sensor preflight topic
if (!_armed) {
_voted_sensors_update.calcAccelInconsistency(_sensor_preflight);
_voted_sensors_update.calcGyroInconsistency(_sensor_preflight);
_voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu);
_voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu);
_sensor_preflight.timestamp = hrt_absolute_time();
_sensor_preflight_pub.publish(_sensor_preflight);
_sensor_preflight_imu.timestamp = hrt_absolute_time();
_sensor_preflight_imu_pub.publish(_sensor_preflight_imu);
}
}
@ -579,6 +603,7 @@ void Sensors::Run()
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleMagnetometer();
_last_config_update = hrt_absolute_time();
} else {
@ -644,6 +669,11 @@ int Sensors::print_status()
{
_voted_sensors_update.printStatus();
if (_vehicle_magnetometer) {
PX4_INFO_RAW("\n");
_vehicle_magnetometer->PrintStatus();
}
if (_vehicle_air_data) {
PX4_INFO_RAW("\n");
_vehicle_air_data->PrintStatus();
@ -696,7 +726,7 @@ The provided functionality includes:
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
sensor drivers must already be running when `sensors` is started.
- Do preflight sensor consistency checks and publish the `sensor_preflight` topic.
- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic.
### Implementation
It runs in its own thread and polls on the currently selected gyro topic.

13
src/lib/mag_compensation/CMakeLists.txt → src/modules/sensors/vehicle_magnetometer/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -31,4 +31,13 @@
#
############################################################################
px4_add_library(mag_compensation MagCompensation.cpp)
px4_add_library(vehicle_magnetometer
VehicleMagnetometer.cpp
VehicleMagnetometer.hpp
)
target_link_libraries(vehicle_magnetometer
PRIVATE
px4_work_queue
sensor_calibration
)

383
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp Normal file
View File

@ -0,0 +1,383 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "VehicleMagnetometer.hpp"
#include <px4_platform_common/log.h>
#include <lib/ecl/geo/geo.h>
namespace sensors
{
using namespace matrix;
using namespace time_literals;
static constexpr int32_t MAG_ROT_VAL_INTERNAL{-1};
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleMagnetometer::VehicleMagnetometer() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
char str[20] {};
for (int mag_index = 0; mag_index < 4; mag_index++) {
// CAL_MAGx_ID
sprintf(str, "CAL_%s%u_ID", "MAG", mag_index);
param_find(str);
// CAL_MAGx_ROT
sprintf(str, "CAL_%s%u_ROT", "MAG", mag_index);
param_find(str);
}
param_find("CAL_MAG_SIDES");
param_find("CAL_MAG_ROT_AUTO");
_voter.set_timeout(SENSOR_TIMEOUT);
_voter.set_equal_value_threshold(1000);
ParametersUpdate(true);
}
VehicleMagnetometer::~VehicleMagnetometer()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleMagnetometer::Start()
{
ScheduleNow();
return true;
}
void VehicleMagnetometer::Stop()
{
Deinit();
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
}
void VehicleMagnetometer::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
// Mag compensation type
MagCompensationType mag_comp_typ = static_cast<MagCompensationType>(_param_mag_comp_typ.get());
if (mag_comp_typ != _mag_comp_type) {
// check mag power compensation type (change battery current subscription instance if necessary)
if (mag_comp_typ == MagCompensationType::Current_inst0 && _mag_comp_type != MagCompensationType::Current_inst0) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
}
if (mag_comp_typ == MagCompensationType::Current_inst1 && _mag_comp_type != MagCompensationType::Current_inst1) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
}
if (mag_comp_typ == MagCompensationType::Throttle) {
_actuator_controls_0_sub = uORB::Subscription{ORB_ID(actuator_controls_0)};
}
}
_mag_comp_type = mag_comp_typ;
}
}
void VehicleMagnetometer::Run()
{
perf_begin(_cycle_perf);
ParametersUpdate();
// check vehicle status for changes to armed state
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode;
if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
_armed = vcontrol_mode.flag_armed;
}
}
if (_mag_comp_type != MagCompensationType::Disabled) {
// update power signal for mag compensation
if (_armed) {
if (_mag_comp_type == MagCompensationType::Throttle) {
actuator_controls_s controls;
if (_actuator_controls_0_sub.update(&controls)) {
for (auto &cal : _calibration) {
cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
}
} else if (_mag_comp_type == MagCompensationType::Current_inst0
|| _mag_comp_type == MagCompensationType::Current_inst1) {
battery_status_s bat_stat;
if (_battery_status_sub.update(&bat_stat)) {
float power = bat_stat.current_a * 0.001f; //current in [kA]
for (auto &cal : _calibration) {
cal.UpdatePower(power);
}
}
}
} else {
for (auto &cal : _calibration) {
cal.UpdatePower(0.f);
}
}
}
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
if (!_calibration[uorb_index].enabled()) {
continue;
}
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
if (_sensor_sub[uorb_index].advertised()) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!_voter.add_new_validator()) {
PX4_ERR("failed to add validator for %s %i", "MAG", uorb_index);
}
}
_advertised[uorb_index] = true;
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
}
} else {
sensor_mag_s report;
updated[uorb_index] = _sensor_sub[uorb_index].update(&report);
if (updated[uorb_index]) {
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_external(report.is_external);
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
}
if (_calibration[uorb_index].enabled()) {
Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
_last_data[uorb_index].device_id = report.device_id;
_last_data[uorb_index].x = vect(0);
_last_data[uorb_index].y = vect(1);
_last_data[uorb_index].z = vect(2);
float mag_array[3] {vect(0), vect(1), vect(2)};
_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
}
}
}
}
// check for the current best sensor
int best_index = 0;
_voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
}
_selected_sensor_sub_index = best_index;
_sensor_sub[_selected_sensor_sub_index].registerCallback();
}
}
if ((_selected_sensor_sub_index >= 0)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
&& updated[_selected_sensor_sub_index]) {
const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index];
_mag_timestamp_sum += mag.timestamp_sample;
_mag_sum += Vector3f{mag.x, mag.y, mag.z};
_mag_sum_count++;
if ((_param_sens_mag_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) {
const Vector3f magnetometer_data = _mag_sum / _mag_sum_count;
const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count;
// reset
_mag_timestamp_sum = 0;
_mag_sum.zero();
_mag_sum_count = 0;
// populate vehicle_magnetometer with primary mag and publish
vehicle_magnetometer_s out{};
out.timestamp_sample = timestamp_sample;
out.device_id = mag.device_id;
magnetometer_data.copyTo(out.magnetometer_ga);
out.timestamp = hrt_absolute_time();
_vehicle_magnetometer_pub.publish(out);
_last_publication_timestamp = out.timestamp;
}
}
// check failover and report
if (_last_failover_count != _voter.failover_count()) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();
if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
if (failover_index != -1) {
const hrt_abstime now = hrt_absolute_time();
if (now - _last_error_message > 3_s) {
mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
"MAG",
failover_index,
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
_last_error_message = now;
}
// reduce priority of failed sensor to the minimum
_priority[failover_index] = 1;
}
}
_last_failover_count = _voter.failover_count();
}
if (!_armed) {
calcMagInconsistency();
}
// reschedule timeout
ScheduleDelayed(100_ms);
perf_end(_cycle_perf);
}
void VehicleMagnetometer::calcMagInconsistency()
{
sensor_preflight_mag_s preflt{};
const sensor_mag_s &primary_mag_report = _last_data[_selected_sensor_sub_index];
const Vector3f primary_mag(primary_mag_report.x, primary_mag_report.y,
primary_mag_report.z); // primary mag field vector
float mag_angle_diff_max = 0.0f; // the maximum angle difference
unsigned check_index = 0; // the number of sensors the primary has been checked against
// Check each sensor against the primary
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
// check that the sensor we are checking against is not the same as the primary
if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) {
// calculate angle to 3D magnetic field vector of the primary sensor
const sensor_mag_s &current_mag_report = _last_data[i];
Vector3f current_mag{current_mag_report.x, current_mag_report.y, current_mag_report.z};
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
// complementary filter to not fail/pass on single outliers
_mag_angle_diff[check_index] *= 0.95f;
_mag_angle_diff[check_index] += 0.05f * angle_error;
mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
// increment the check index
check_index++;
}
// check to see if the maximum number of checks has been reached and break
if (check_index >= 2) {
break;
}
}
// get the vector length of the largest difference and write to the combined sensor struct
// will be zero if there is only one magnetometer and hence nothing to compare
preflt.mag_inconsistency_angle = mag_angle_diff_max;
preflt.timestamp = hrt_absolute_time();
_sensor_preflight_mag_pub.publish(preflt);
}
void VehicleMagnetometer::PrintStatus()
{
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("selected magnetometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id,
_selected_sensor_sub_index);
}
_voter.print();
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_advertised[i] && (_priority[i] > 0)) {
_calibration[i].PrintStatus();
}
}
}
}; // namespace sensors

140
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp Normal file
View File

@ -0,0 +1,140 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_preflight_mag.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_magnetometer.h>
namespace sensors
{
class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleMagnetometer();
~VehicleMagnetometer() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
void calcMagInconsistency();
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<sensor_preflight_mag_s> _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)};
uORB::Publication<vehicle_magnetometer_s> _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_mag), 0},
{this, ORB_ID(sensor_mag), 1},
{this, ORB_ID(sensor_mag), 2},
{this, ORB_ID(sensor_mag), 3}
};
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
// Magnetometer interference compensation
enum class MagCompensationType {
Disabled = 0,
Throttle,
Current_inst0,
Current_inst1
};
MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_publication_timestamp{0};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint64_t _mag_timestamp_sum{0};
matrix::Vector3f _mag_sum{};
int _mag_sum_count{0};
sensor_mag_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
uint8_t _priority[MAX_SENSOR_COUNT] {};
int8_t _selected_sensor_sub_index{-1};
bool _armed{false}; /**< arming status of the vehicle */
DEFINE_PARAMETERS(
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
)
};
}; // namespace sensors

0
src/lib/mag_compensation/python/mag_compensation.py → src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py
View File

289
src/modules/sensors/voted_sensors_update.cpp
View File

@ -39,32 +39,20 @@
#include "voted_sensors_update.h"
#include <lib/conversion/rotation.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#define MAG_ROT_VAL_INTERNAL -1
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
using namespace sensors;
using namespace matrix;
using namespace time_literals;
using math::radians;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
ModuleParams(nullptr),
_vehicle_imu_sub(vehicle_imu_sub),
_parameters(parameters),
_hil_enabled(hil_enabled),
_mag_compensator(this)
_hil_enabled(hil_enabled)
{
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
_gyro.voter.set_timeout(500000);
_accel.voter.set_timeout(500000);
@ -87,26 +75,12 @@ void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(_gyro, GYRO_COUNT_MAX);
initSensorClass(_accel, ACCEL_COUNT_MAX);
initSensorClass(_mag, MAG_COUNT_MAX);
}
void VotedSensorsUpdate::parametersUpdate()
{
updateParams();
/* fine tune board offset */
const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}};
_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
// initialze all mag rotations with the board rotation in case there is no calibration data available
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
_mag_rotation[topic_instance] = _board_rotation;
}
/* set offset parameters to new values */
bool failed = false;
// run through all IMUs
for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) {
uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
@ -141,159 +115,6 @@ void VotedSensorsUpdate::parametersUpdate()
}
}
}
/* run through all mag sensors
* Because we store the device id in _mag_device_id, we need to get the id via uorb topic since
* the DevHandle method does not work on POSIX.
*/
/* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers,
* and not all uORB instances have to be published yet at the initial call of parametersUpdate()
*/
for (int i = 0; i < MAG_COUNT_MAX; i++) {
_mag.enabled[i] = true;
}
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX
&& topic_instance < _mag.subscription_count; ++topic_instance) {
sensor_mag_s report{};
if (!_mag.subscription[topic_instance].copy(&report)) {
continue;
}
uint32_t topic_device_id = report.device_id;
bool is_external = report.is_external;
_mag_device_id[topic_instance] = topic_device_id;
// find the driver handle that matches the topic_device_id
int fd = -1;
char str[30] {};
for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) {
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
fd = px4_open(str, O_RDWR);
if (fd < 0) {
/* the driver is not running, continue with the next */
continue;
}
uint32_t driver_device_id = (uint32_t)px4_ioctl(fd, DEVIOCGDEVICEID, 0);
if (driver_device_id == topic_device_id) {
break; // we found the matching driver
} else {
px4_close(fd);
}
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < MAG_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id = 0;
failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_MAG%u_EN", i);
int32_t device_enabled = 1;
failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
if (failed) {
continue;
}
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == _mag_device_id[topic_instance]) {
_mag.enabled[topic_instance] = (device_enabled == 1);
_mag.power_compensation[topic_instance] = {_parameters.mag_comp_paramX[i], _parameters.mag_comp_paramY[i], _parameters.mag_comp_paramZ[i]};
// the mags that were published after the initial parameterUpdate
// would be given the priority even if disabled. Reset it to 0 in this case
if (!_mag.enabled[topic_instance]) {
_mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED;
}
mag_calibration_s mscale{};
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_offset));
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_offset));
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_offset));
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_scale));
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_scale));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_scale));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
int32_t mag_rot = 0;
param_get(param_find(str), &mag_rot);
if (is_external) {
/* check if this mag is still set as internal, otherwise leave untouched */
if (mag_rot < 0) {
/* it was marked as internal, change to external with no rotation */
mag_rot = 0;
param_set_no_notification(param_find(str), &mag_rot);
}
} else {
/* mag is internal - reset param to -1 to indicate internal mag */
if (mag_rot != MAG_ROT_VAL_INTERNAL) {
mag_rot = MAG_ROT_VAL_INTERNAL;
param_set_no_notification(param_find(str), &mag_rot);
}
}
/* now get the mag rotation */
if (mag_rot >= 0) {
// Set external magnetometers to use the parameter value
_mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot);
} else {
// Set internal magnetometers to use the board rotation
_mag_rotation[topic_instance] = _board_rotation;
}
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
config_ok = (px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale) == 0);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
}
}
break;
}
}
px4_close(fd);
}
}
void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
@ -398,51 +219,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
}
}
void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
{
for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) {
sensor_mag_s mag_report;
if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) {
// First publication with data
if (_mag.priority[uorb_index] == 0) {
_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
}
Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
//throttle-/current-based mag compensation
_mag_compensator.calculate_mag_corrected(vect, _mag.power_compensation[uorb_index]);
vect = _mag_rotation[uorb_index] * vect;
_last_magnetometer[uorb_index].timestamp = mag_report.timestamp;
_last_magnetometer[uorb_index].magnetometer_ga[0] = vect(0);
_last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1);
_last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2);
_mag.voter.put(uorb_index, mag_report.timestamp, _last_magnetometer[uorb_index].magnetometer_ga, mag_report.error_count,
_mag.priority[uorb_index]);
}
}
int best_index;
_mag.voter.get_best(hrt_absolute_time(), &best_index);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
if (best_index >= 0) {
magnetometer = _last_magnetometer[best_index];
_mag.last_best_vote = (uint8_t)best_index;
if (_selection.mag_device_id != _mag_device_id[best_index]) {
_selection_changed = true;
_selection.mag_device_id = _mag_device_id[best_index];
}
}
}
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
{
if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
@ -494,8 +270,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
if (type == subsystem_info_s::SUBSYSTEM_TYPE_MAG) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
}
_info.timestamp = hrt_absolute_time();
@ -558,16 +332,11 @@ void VotedSensorsUpdate::printStatus()
PX4_INFO_RAW("\n");
PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote);
_accel.voter.print();
PX4_INFO_RAW("\n");
PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote);
_mag.voter.print();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer)
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
{
imuPoll(raw);
magPoll(magnetometer);
// publish sensor selection if changed
if (_selection_changed) {
@ -588,8 +357,7 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
}
}
void
VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
{
float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
@ -638,7 +406,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
}
}
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
{
float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
@ -686,48 +454,3 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq);
}
}
void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
{
Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector
float mag_angle_diff_max = 0.0f; // the maximum angle difference
unsigned check_index = 0; // the number of sensors the primary has been checked against
// Check each sensor against the primary
for (int i = 0; i < _mag.subscription_count; i++) {
// check that the sensor we are checking against is not the same as the primary
if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
// calculate angle to 3D magnetic field vector of the primary sensor
Vector3f current_mag(_last_magnetometer[i].magnetometer_ga);
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
// complementary filter to not fail/pass on single outliers
_mag_angle_diff[check_index] *= 0.95f;
_mag_angle_diff[check_index] += 0.05f * angle_error;
mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
// increment the check index
check_index++;
}
// check to see if the maximum number of checks has been reached and break
if (check_index >= 2) {
break;
}
}
// get the vector length of the largest difference and write to the combined sensor struct
// will be zero if there is only one magnetometer and hence nothing to compare
preflt.mag_inconsistency_angle = mag_angle_diff_max;
}
void VotedSensorsUpdate::update_mag_comp_armed(bool armed)
{
_mag_compensator.update_armed_flag(armed);
}
void VotedSensorsUpdate::update_mag_comp_power(float power)
{
_mag_compensator.update_power(power);
}

75
src/modules/sensors/voted_sensors_update.h
View File

@ -39,35 +39,33 @@
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
#include "data_validator/DataValidator.hpp"
#include "data_validator/DataValidatorGroup.hpp"
#include <drivers/drv_mag.h>
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/mag_compensation/MagCompensation.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
#include "common.h"
namespace sensors
{
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX);
/**
** class VotedSensorsUpdate
*
@ -80,8 +78,7 @@ public:
* @param parameters parameter values. These do not have to be initialized when constructing this object.
* Only when calling init(), they have to be initialized.
*/
VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
/**
* initialize subscriptions etc.
@ -105,7 +102,7 @@ public:
/**
* read new sensor data
*/
void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer);
void sensorsPoll(sensor_combined_s &raw);
/**
* set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
@ -116,34 +113,22 @@ public:
/**
* Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
*/
void calcAccelInconsistency(sensor_preflight_s &preflt);
void calcAccelInconsistency(sensor_preflight_imu_s &preflt);
/**
* Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
*/
void calcGyroInconsistency(sensor_preflight_s &preflt);
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
void calcMagInconsistency(sensor_preflight_s &preflt);
/**
* Update armed flag for mag compensation.
*/
void update_mag_comp_armed(bool armed);
/**
* Update power signal for mag compensation.
* power: either throttle value [0,1] or current measurement in [kA]
*/
void update_mag_comp_power(float power);
void calcGyroInconsistency(sensor_preflight_imu_s &preflt);
private:
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
struct SensorData {
SensorData() = delete;
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
DataValidatorGroup voter{1};
@ -151,9 +136,8 @@ private:
ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
uint8_t last_best_vote{0}; /**< index of the latest best vote */
uint8_t subscription_count{0};
bool enabled[SENSOR_COUNT_MAX] {true, true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false, false};
matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
bool enabled[SENSOR_COUNT_MAX] {true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false};
};
void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
@ -166,14 +150,6 @@ private:
*/
void imuPoll(sensor_combined_s &raw);
/**
* Poll the magnetometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void magPoll(vehicle_magnetometer_s &magnetometer);
/**
* Check & handle failover of a sensor
* @return true if a switch occured (could be for a non-critical reason)
@ -182,7 +158,6 @@ private:
SensorData _accel{ORB_ID::sensor_accel};
SensorData _gyro{ORB_ID::sensor_gyro};
SensorData _mag{ORB_ID::sensor_mag};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
@ -198,26 +173,16 @@ private:
};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */
matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {}; /**< rotation matrix for the orientation that the external mag0 is mounted */
const Parameters &_parameters;
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */
float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
/* Magnetometer interference compensation */
MagCompensator _mag_compensator;
uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */

8
src/modules/simulator/simulator.cpp
View File

@ -64,12 +64,6 @@ void Simulator::parameters_update(bool force)
}
}
void Simulator::print_status()
{
PX4_INFO("magnetometer");
_px4_mag.print_status();
}
int Simulator::start(int argc, char *argv[])
{
_instance = new Simulator();
@ -154,7 +148,7 @@ int simulator_main(int argc, char *argv[])
return 1;
} else {
Simulator::getInstance()->print_status();
PX4_INFO("running");
}
} else {

2
src/modules/simulator/simulator.h
View File

@ -127,8 +127,6 @@ public:
bool has_initialized() { return _has_initialized.load(); }
#endif
void print_status();
private:
Simulator() : ModuleParams(nullptr)
{