forked from Archive/PX4-Autopilot
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:
parent
ad148af2fd
commit
971b1e4b4d
|
@ -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"'
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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.
|
|
@ -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.
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -669,6 +669,4 @@ MPU9250::print_status()
|
|||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_bad_registers);
|
||||
perf_print_counter(_duplicates);
|
||||
|
||||
_mag.print_status();
|
||||
}
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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 *
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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 ×tamp_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);
|
||||
}
|
||||
|
|
|
@ -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 ×tamp_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};
|
||||
|
||||
|
|
|
@ -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 ¶m_vect)
|
||||
{
|
||||
if (_armed) {
|
||||
mag = mag + param_vect * _power;
|
||||
}
|
||||
}
|
|
@ -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 ¶m_vect);
|
||||
|
||||
private:
|
||||
float _power{0};
|
||||
bool _armed{false};
|
||||
};
|
|
@ -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);
|
|
@ -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 ---- */
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
)
|
||||
|
|
|
@ -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 */
|
|
@ -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 ¶meter_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 ¶meter_handles, Parameters ¶meters)
|
||||
{
|
||||
/* 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, ¶meters.air_cmodel);
|
||||
param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length);
|
||||
param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm);
|
||||
}
|
||||
|
||||
} /* namespace sensors */
|
|
@ -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 ¶meter_handles);
|
||||
|
||||
|
||||
/**
|
||||
* Read out the parameters using the handles into the parameters struct.
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
void update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters);
|
||||
|
||||
} /* namespace sensors */
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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
|
||||
)
|
|
@ -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(¶m_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 ¤t_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
|
|
@ -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
|
|
@ -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 ¶meters, 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);
|
||||
}
|
||||
|
|
|
@ -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 ¶meters, 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 */
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -127,8 +127,6 @@ public:
|
|||
bool has_initialized() { return _has_initialized.load(); }
|
||||
#endif
|
||||
|
||||
void print_status();
|
||||
|
||||
private:
|
||||
Simulator() : ModuleParams(nullptr)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue