fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move mag aggregation to new VehicleMagnetometer WorkItem

Browse Source 
- purge all reminaing magnetometer IOCTL usage
 - mag calibration add off diagonal (soft iron) scale factors
This commit is contained in:
Daniel Agar
2020-08-21 10:12:13 -04:00
parent ad148af2fd
commit 971b1e4b4d
72 changed files with 870 additions and 1134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+6 -62
View File
@@ -87,11 +87,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_mag == 1) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
@@ -99,29 +94,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
failed = true;
}
// TODO: highest priority mag
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
@@ -132,10 +112,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- ACCEL ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
@@ -143,63 +119,31 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
int32_t device_id = -1;
if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- BARO ---- */
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
+3 -3
View File
@@ -38,7 +38,7 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -47,9 +47,9 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_imu_s &sensors = sensors_sub.get();
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
+3 -3
View File
@@ -40,7 +40,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_mag.h>
// return false if the magnetomer measurements are inconsistent
bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -49,9 +49,9 @@ bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
bool pass = false; // flag for result of checks
// get the sensor preflight data
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_mag_s &sensors = sensors_sub.get();
if (sensors.timestamp == 0) {
// can happen if not advertised (yet)
src/modules/ekf2/ekf2_main.cpp
+5 -7
View File
@@ -886,8 +886,8 @@ void Ekf2::Run()
// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
// and notification events
// Check if there has been a persistant change in magnetometer ID
if (_sensor_selection.mag_device_id != 0
&& (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
if (magnetometer.device_id != 0
&& (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
if (_invalid_mag_id_count < 200) {
_invalid_mag_id_count++;
@@ -908,7 +908,7 @@ void Ekf2::Run()
_param_ekf2_magbias_y.commit_no_notification();
_param_ekf2_magbias_z.set(0.f);
_param_ekf2_magbias_z.commit_no_notification();
_param_ekf2_magbias_id.set(_sensor_selection.mag_device_id);
_param_ekf2_magbias_id.set(magnetometer.device_id);
_param_ekf2_magbias_id.commit();
_invalid_mag_id_count = 0;
@@ -1510,7 +1510,7 @@ void Ekf2::Run()
bias.accel_device_id = _sensor_selection.accel_device_id;
}
bias.mag_device_id = _sensor_selection.mag_device_id;
bias.mag_device_id = _param_ekf2_magbias_id.get();
// In-run bias estimates
_ekf.getGyroBias().copyTo(bias.gyro_bias);
@@ -1632,8 +1632,7 @@ void Ekf2::Run()
// Check and save the last valid calibration when we are disarmed
if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& (status.filter_fault_flags == 0)
&& (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) {
&& (status.filter_fault_flags == 0)) {
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
@@ -1642,7 +1641,6 @@ void Ekf2::Run()
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
}
}
publish_wind_estimate(now);
src/modules/logger/logged_topics.cpp
+2 -1
View File
@@ -79,7 +79,8 @@ void LoggedTopics::add_default_topics()
add_topic("safety");
add_topic("sensor_combined");
add_topic("sensor_correction");
add_topic("sensor_preflight", 200);
add_topic("sensor_preflight_imu", 200);
add_topic("sensor_preflight_mag", 500);
add_topic("sensor_selection");
add_topic("system_power", 500);
add_topic("tecs_status", 200);
src/modules/sensors/CMakeLists.txt
+2 -2
View File
@@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_air_data)
add_subdirectory(vehicle_imu)
add_subdirectory(vehicle_magnetometer)
px4_add_module(
MODULE modules__sensors
@@ -45,7 +46,6 @@ px4_add_module(
SRCS
voted_sensors_update.cpp
sensors.cpp
parameters.cpp
DEPENDS
airspeed
conversion
@@ -58,5 +58,5 @@ px4_add_module(
vehicle_angular_velocity
vehicle_air_data
vehicle_imu
mag_compensation
vehicle_magnetometer
)
src/modules/sensors/common.h
-52
View File
@@ -1,52 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file common.h
* common definitions used in sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
namespace sensors
{
constexpr uint8_t MAG_COUNT_MAX = 4;
constexpr uint8_t GYRO_COUNT_MAX = 3;
constexpr uint8_t ACCEL_COUNT_MAX = 3;
constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX));
} /* namespace sensors */
src/modules/sensors/data_validator/tests/tests_common.cpp
+1 -1
View File
@@ -100,4 +100,4 @@ void fill_validator_with_samples(DataValidator *validator,
*timestamp_io = timestamp;
*value_io = val;
}
}
src/modules/sensors/parameters.cpp
-147
View File
@@ -1,147 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file parameters.cpp
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
namespace sensors
{
void initialize_parameter_handles(ParameterHandles &parameter_handles)
{
/* Differential pressure offset */
parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
/* rotations */
parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* mag compensation */
parameter_handles.mag_comp_type = param_find("CAL_MAG_COMP_TYP");
parameter_handles.mag_comp_paramX[0] = param_find("CAL_MAG0_XCOMP");
parameter_handles.mag_comp_paramX[1] = param_find("CAL_MAG1_XCOMP");
parameter_handles.mag_comp_paramX[2] = param_find("CAL_MAG2_XCOMP");
parameter_handles.mag_comp_paramX[3] = param_find("CAL_MAG3_XCOMP");
parameter_handles.mag_comp_paramY[0] = param_find("CAL_MAG0_YCOMP");
parameter_handles.mag_comp_paramY[1] = param_find("CAL_MAG1_YCOMP");
parameter_handles.mag_comp_paramY[2] = param_find("CAL_MAG2_YCOMP");
parameter_handles.mag_comp_paramY[3] = param_find("CAL_MAG3_YCOMP");
parameter_handles.mag_comp_paramZ[0] = param_find("CAL_MAG0_ZCOMP");
parameter_handles.mag_comp_paramZ[1] = param_find("CAL_MAG1_ZCOMP");
parameter_handles.mag_comp_paramZ[2] = param_find("CAL_MAG2_ZCOMP");
parameter_handles.mag_comp_paramZ[3] = param_find("CAL_MAG3_ZCOMP");
parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
(void)param_find("BAT_V_DIV");
(void)param_find("BAT_A_PER_V");
(void)param_find("CAL_ACC0_ID");
(void)param_find("CAL_GYRO0_ID");
(void)param_find("CAL_MAG0_ID");
(void)param_find("CAL_MAG1_ID");
(void)param_find("CAL_MAG2_ID");
(void)param_find("CAL_MAG3_ID");
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("CAL_MAG3_ROT");
(void)param_find("CAL_MAG_SIDES");
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");
(void)param_find("TRIG_MODE");
(void)param_find("UAVCAN_ENABLE");
// Parameters controlling the on-board sensor thermal calibrator
(void)param_find("SYS_CAL_TDEL");
(void)param_find("SYS_CAL_TMAX");
(void)param_find("SYS_CAL_TMIN");
}
void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
/* Airspeed offset */
param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa));
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_get(parameter_handles.board_rotation, &(parameters.board_rotation));
param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0]));
param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
param_get(parameter_handles.mag_comp_type, &(parameters.mag_comp_type));
param_get(parameter_handles.mag_comp_paramX[0], &(parameters.mag_comp_paramX[0]));
param_get(parameter_handles.mag_comp_paramX[1], &(parameters.mag_comp_paramX[1]));
param_get(parameter_handles.mag_comp_paramX[2], &(parameters.mag_comp_paramX[2]));
param_get(parameter_handles.mag_comp_paramX[3], &(parameters.mag_comp_paramX[3]));
param_get(parameter_handles.mag_comp_paramY[0], &(parameters.mag_comp_paramY[0]));
param_get(parameter_handles.mag_comp_paramY[1], &(parameters.mag_comp_paramY[1]));
param_get(parameter_handles.mag_comp_paramY[2], &(parameters.mag_comp_paramY[2]));
param_get(parameter_handles.mag_comp_paramY[3], &(parameters.mag_comp_paramY[3]));
param_get(parameter_handles.mag_comp_paramZ[0], &(parameters.mag_comp_paramZ[0]));
param_get(parameter_handles.mag_comp_paramZ[1], &(parameters.mag_comp_paramZ[1]));
param_get(parameter_handles.mag_comp_paramZ[2], &(parameters.mag_comp_paramZ[2]));
param_get(parameter_handles.mag_comp_paramZ[3], &(parameters.mag_comp_paramZ[3]));
param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
}
} /* namespace sensors */
src/modules/sensors/parameters.h
-103
View File
@@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file parameters.h
*
* defines the list of parameters that are used within the sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include <lib/parameters/param.h>
namespace sensors
{
struct Parameters {
float diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
int32_t board_rotation;
float board_offset[3];
//parameters for current/throttle-based mag compensation
int32_t mag_comp_type;
float mag_comp_paramX[4];
float mag_comp_paramY[4];
float mag_comp_paramZ[4];
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
};
struct ParameterHandles {
param_t diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_t board_rotation;
param_t board_offset[3];
param_t mag_comp_type;
param_t mag_comp_paramX[4];
param_t mag_comp_paramY[4];
param_t mag_comp_paramZ[4];
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;
};
/**
* initialize ParameterHandles struct
*/
void initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on success, <0 on error
*/
void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
} /* namespace sensors */
src/modules/sensors/sensor_params_mag.c
+29
View File
@@ -62,3 +62,32 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
* @group Sensors
*/
PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
/**
* Type of magnetometer compensation
*
* @value 0 Disabled
* @value 1 Throttle-based compensation
* @value 2 Current-based compensation (battery_status instance 0)
* @value 3 Current-based compensation (battery_status instance 1)
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);
/**
* Magnetometer max rate.
*
* Magnetometer data maximum publication rate. This is an upper bound,
* actual magnetometer data rate is still dependant on the sensor.
*
* @min 1
* @max 200
* @group Sensors
* @unit Hz
*
* @reboot_required true
*
*/
PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);
src/modules/sensors/sensor_params_mag0.c
+25 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
src/modules/sensors/sensor_params_mag1.c
+25 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
src/modules/sensors/sensor_params_mag2.c
+25 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
src/modules/sensors/sensor_params_mag3.c
+25 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f);
/**
* Magnetometer X-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0f);
/**
* Magnetometer Y-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0f);
/**
* Magnetometer Z-axis off diagonal factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
src/modules/sensors/sensors.cpp
+110 -80
View File
@@ -45,7 +45,6 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/airspeed/airspeed.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@@ -65,19 +64,17 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/battery_status.h>
#include "parameters.h"
#include "voted_sensors_update.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
using namespace sensors;
using namespace time_literals;
@@ -116,10 +113,9 @@ private:
hrt_abstime _last_config_update{0};
hrt_abstime _sensor_combined_prev_timestamp{0};
hrt_abstime _magnetometer_prev_timestamp{0};
sensor_combined_s _sensor_combined{};
sensor_preflight_s _sensor_preflight{};
sensor_preflight_imu_s _sensor_preflight_imu{};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
{this, ORB_ID(vehicle_imu), 0},
@@ -127,26 +123,14 @@ private:
{this, ORB_ID(vehicle_imu), 2}
};
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; /**< battery_status instance 0 subscription */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
uORB::Publication<sensor_preflight_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
enum class MagCompensationType {
Disabled = 0,
Throttle,
Current_inst0,
Current_inst1
};
MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
uORB::Publication<sensor_preflight_imu_s> _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)};
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -161,14 +145,35 @@ private:
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
Parameters _parameters{}; /**< local copies of interesting parameters */
ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */
struct Parameters {
float diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
float diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
} _parameters{}; /**< local copies of interesting parameters */
struct ParameterHandles {
param_t diff_pres_offset_pa;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_t diff_pres_analog_scale;
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_t air_cmodel;
param_t air_tube_length;
param_t air_tube_diameter_mm;
} _parameter_handles{}; /**< handles for interesting parameters */
VotedSensorsUpdate _voted_sensors_update;
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData *_vehicle_air_data{nullptr};
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -202,9 +207,11 @@ private:
void InitializeVehicleAirData();
void InitializeVehicleIMU();
void InitializeVehicleMagnetometer();
DEFINE_PARAMETERS(
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
)
};
@@ -213,9 +220,39 @@ Sensors::Sensors(bool hil_enabled) :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_hil_enabled(hil_enabled),
_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
_voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub)
_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
{
initialize_parameter_handles(_parameter_handles);
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
param_find("BAT_V_DIV");
param_find("BAT_A_PER_V");
param_find("CAL_ACC0_ID");
param_find("CAL_GYRO0_ID");
param_find("SENS_BOARD_ROT");
param_find("SENS_BOARD_X_OFF");
param_find("SENS_BOARD_Y_OFF");
param_find("SENS_BOARD_Z_OFF");
param_find("SYS_PARAM_VER");
param_find("SYS_AUTOSTART");
param_find("SYS_AUTOCONFIG");
param_find("TRIG_MODE");
param_find("UAVCAN_ENABLE");
// Parameters controlling the on-board sensor thermal calibrator
param_find("SYS_CAL_TDEL");
param_find("SYS_CAL_TMAX");
param_find("SYS_CAL_TMIN");
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
@@ -239,6 +276,11 @@ Sensors::~Sensors()
delete _vehicle_air_data;
}
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Stop();
delete _vehicle_magnetometer;
}
for (auto &vehicle_imu : _vehicle_imu_list) {
if (vehicle_imu) {
vehicle_imu->Stop();
@@ -265,8 +307,15 @@ int Sensors::parameters_update()
return 0;
}
/* read the parameter values into _parameters */
update_parameters(_parameter_handles, _parameters);
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel);
param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length);
param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
_voted_sensors_update.parametersUpdate();
@@ -474,6 +523,21 @@ void Sensors::InitializeVehicleIMU()
}
}
void Sensors::InitializeVehicleMagnetometer()
{
if (_param_sys_has_mag.get()) {
if (_vehicle_magnetometer == nullptr) {
if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
_vehicle_magnetometer = new VehicleMagnetometer();
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Start();
}
}
}
}
}
void Sensors::Run()
{
if (should_exit()) {
@@ -490,6 +554,7 @@ void Sensors::Run()
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleMagnetometer();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
}
@@ -505,57 +570,16 @@ void Sensors::Run()
if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
_armed = vcontrol_mode.flag_armed;
_voted_sensors_update.update_mag_comp_armed(_armed);
}
//check mag power compensation type (change battery current subscription instance if necessary)
if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0
&& _mag_comp_type != MagCompensationType::Current_inst0) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
}
if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1
&& _mag_comp_type != MagCompensationType::Current_inst1) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
}
_mag_comp_type = (MagCompensationType)_parameters.mag_comp_type;
//update power signal for mag compensation
if (_mag_comp_type == MagCompensationType::Throttle) {
actuator_controls_s controls {};
if (_actuator_ctrl_0_sub.update(&controls)) {
_voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
} else if (_mag_comp_type == MagCompensationType::Current_inst0
|| _mag_comp_type == MagCompensationType::Current_inst1) {
battery_status_s bat_stat {};
if (_battery_status_sub.update(&bat_stat)) {
_voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA]
}
}
}
vehicle_magnetometer_s magnetometer{};
_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
_voted_sensors_update.sensorsPoll(_sensor_combined);
// check analog airspeed
adc_poll();
diff_pres_poll();
if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
_magnetometer_pub.publish(magnetometer);
_magnetometer_prev_timestamp = magnetometer.timestamp;
if (!_armed) {
_voted_sensors_update.calcMagInconsistency(_sensor_preflight);
}
}
if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
@@ -565,11 +589,11 @@ void Sensors::Run()
// If the the vehicle is disarmed calculate the length of the maximum difference between
// IMU units as a consistency metric and publish to the sensor preflight topic
if (!_armed) {
_voted_sensors_update.calcAccelInconsistency(_sensor_preflight);
_voted_sensors_update.calcGyroInconsistency(_sensor_preflight);
_voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu);
_voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu);
_sensor_preflight.timestamp = hrt_absolute_time();
_sensor_preflight_pub.publish(_sensor_preflight);
_sensor_preflight_imu.timestamp = hrt_absolute_time();
_sensor_preflight_imu_pub.publish(_sensor_preflight_imu);
}
}
@@ -579,6 +603,7 @@ void Sensors::Run()
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleMagnetometer();
_last_config_update = hrt_absolute_time();
} else {
@@ -644,6 +669,11 @@ int Sensors::print_status()
{
_voted_sensors_update.printStatus();
if (_vehicle_magnetometer) {
PX4_INFO_RAW("\n");
_vehicle_magnetometer->PrintStatus();
}
if (_vehicle_air_data) {
PX4_INFO_RAW("\n");
_vehicle_air_data->PrintStatus();
@@ -696,7 +726,7 @@ The provided functionality includes:
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
sensor drivers must already be running when `sensors` is started.
- Do preflight sensor consistency checks and publish the `sensor_preflight` topic.
- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic.
### Implementation
It runs in its own thread and polls on the currently selected gyro topic.
src/modules/sensors/vehicle_magnetometer/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(vehicle_magnetometer
VehicleMagnetometer.cpp
VehicleMagnetometer.hpp
)
target_link_libraries(vehicle_magnetometer
PRIVATE
px4_work_queue
sensor_calibration
)
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+383
View File
@@ -0,0 +1,383 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "VehicleMagnetometer.hpp"
#include <px4_platform_common/log.h>
#include <lib/ecl/geo/geo.h>
namespace sensors
{
using namespace matrix;
using namespace time_literals;
static constexpr int32_t MAG_ROT_VAL_INTERNAL{-1};
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleMagnetometer::VehicleMagnetometer() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
char str[20] {};
for (int mag_index = 0; mag_index < 4; mag_index++) {
// CAL_MAGx_ID
sprintf(str, "CAL_%s%u_ID", "MAG", mag_index);
param_find(str);
// CAL_MAGx_ROT
sprintf(str, "CAL_%s%u_ROT", "MAG", mag_index);
param_find(str);
}
param_find("CAL_MAG_SIDES");
param_find("CAL_MAG_ROT_AUTO");
_voter.set_timeout(SENSOR_TIMEOUT);
_voter.set_equal_value_threshold(1000);
ParametersUpdate(true);
}
VehicleMagnetometer::~VehicleMagnetometer()
{
Stop();
perf_free(_cycle_perf);
}
bool VehicleMagnetometer::Start()
{
ScheduleNow();
return true;
}
void VehicleMagnetometer::Stop()
{
Deinit();
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
}
void VehicleMagnetometer::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
// Mag compensation type
MagCompensationType mag_comp_typ = static_cast<MagCompensationType>(_param_mag_comp_typ.get());
if (mag_comp_typ != _mag_comp_type) {
// check mag power compensation type (change battery current subscription instance if necessary)
if (mag_comp_typ == MagCompensationType::Current_inst0 && _mag_comp_type != MagCompensationType::Current_inst0) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
}
if (mag_comp_typ == MagCompensationType::Current_inst1 && _mag_comp_type != MagCompensationType::Current_inst1) {
_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
}
if (mag_comp_typ == MagCompensationType::Throttle) {
_actuator_controls_0_sub = uORB::Subscription{ORB_ID(actuator_controls_0)};
}
}
_mag_comp_type = mag_comp_typ;
}
}
void VehicleMagnetometer::Run()
{
perf_begin(_cycle_perf);
ParametersUpdate();
// check vehicle status for changes to armed state
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode;
if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
_armed = vcontrol_mode.flag_armed;
}
}
if (_mag_comp_type != MagCompensationType::Disabled) {
// update power signal for mag compensation
if (_armed) {
if (_mag_comp_type == MagCompensationType::Throttle) {
actuator_controls_s controls;
if (_actuator_controls_0_sub.update(&controls)) {
for (auto &cal : _calibration) {
cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
}
} else if (_mag_comp_type == MagCompensationType::Current_inst0
|| _mag_comp_type == MagCompensationType::Current_inst1) {
battery_status_s bat_stat;
if (_battery_status_sub.update(&bat_stat)) {
float power = bat_stat.current_a * 0.001f; //current in [kA]
for (auto &cal : _calibration) {
cal.UpdatePower(power);
}
}
}
} else {
for (auto &cal : _calibration) {
cal.UpdatePower(0.f);
}
}
}
bool updated[MAX_SENSOR_COUNT] {};
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
if (!_calibration[uorb_index].enabled()) {
continue;
}
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
if (_sensor_sub[uorb_index].advertised()) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
if (!_voter.add_new_validator()) {
PX4_ERR("failed to add validator for %s %i", "MAG", uorb_index);
}
}
_advertised[uorb_index] = true;
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
}
} else {
sensor_mag_s report;
updated[uorb_index] = _sensor_sub[uorb_index].update(&report);
if (updated[uorb_index]) {
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_external(report.is_external);
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
}
if (_calibration[uorb_index].enabled()) {
Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
_last_data[uorb_index].device_id = report.device_id;
_last_data[uorb_index].x = vect(0);
_last_data[uorb_index].y = vect(1);
_last_data[uorb_index].z = vect(2);
float mag_array[3] {vect(0), vect(1), vect(2)};
_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
}
}
}
}
// check for the current best sensor
int best_index = 0;
_voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
if (_selected_sensor_sub_index != best_index) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
}
_selected_sensor_sub_index = best_index;
_sensor_sub[_selected_sensor_sub_index].registerCallback();
}
}
if ((_selected_sensor_sub_index >= 0)
&& (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
&& updated[_selected_sensor_sub_index]) {
const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index];
_mag_timestamp_sum += mag.timestamp_sample;
_mag_sum += Vector3f{mag.x, mag.y, mag.z};
_mag_sum_count++;
if ((_param_sens_mag_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) {
const Vector3f magnetometer_data = _mag_sum / _mag_sum_count;
const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count;
// reset
_mag_timestamp_sum = 0;
_mag_sum.zero();
_mag_sum_count = 0;
// populate vehicle_magnetometer with primary mag and publish
vehicle_magnetometer_s out{};
out.timestamp_sample = timestamp_sample;
out.device_id = mag.device_id;
magnetometer_data.copyTo(out.magnetometer_ga);
out.timestamp = hrt_absolute_time();
_vehicle_magnetometer_pub.publish(out);
_last_publication_timestamp = out.timestamp;
}
}
// check failover and report
if (_last_failover_count != _voter.failover_count()) {
uint32_t flags = _voter.failover_state();
int failover_index = _voter.failover_index();
if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
if (failover_index != -1) {
const hrt_abstime now = hrt_absolute_time();
if (now - _last_error_message > 3_s) {
mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
"MAG",
failover_index,
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
_last_error_message = now;
}
// reduce priority of failed sensor to the minimum
_priority[failover_index] = 1;
}
}
_last_failover_count = _voter.failover_count();
}
if (!_armed) {
calcMagInconsistency();
}
// reschedule timeout
ScheduleDelayed(100_ms);
perf_end(_cycle_perf);
}
void VehicleMagnetometer::calcMagInconsistency()
{
sensor_preflight_mag_s preflt{};
const sensor_mag_s &primary_mag_report = _last_data[_selected_sensor_sub_index];
const Vector3f primary_mag(primary_mag_report.x, primary_mag_report.y,
primary_mag_report.z); // primary mag field vector
float mag_angle_diff_max = 0.0f; // the maximum angle difference
unsigned check_index = 0; // the number of sensors the primary has been checked against
// Check each sensor against the primary
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
// check that the sensor we are checking against is not the same as the primary
if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) {
// calculate angle to 3D magnetic field vector of the primary sensor
const sensor_mag_s &current_mag_report = _last_data[i];
Vector3f current_mag{current_mag_report.x, current_mag_report.y, current_mag_report.z};
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
// complementary filter to not fail/pass on single outliers
_mag_angle_diff[check_index] *= 0.95f;
_mag_angle_diff[check_index] += 0.05f * angle_error;
mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
// increment the check index
check_index++;
}
// check to see if the maximum number of checks has been reached and break
if (check_index >= 2) {
break;
}
}
// get the vector length of the largest difference and write to the combined sensor struct
// will be zero if there is only one magnetometer and hence nothing to compare
preflt.mag_inconsistency_angle = mag_angle_diff_max;
preflt.timestamp = hrt_absolute_time();
_sensor_preflight_mag_pub.publish(preflt);
}
void VehicleMagnetometer::PrintStatus()
{
if (_selected_sensor_sub_index >= 0) {
PX4_INFO("selected magnetometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id,
_selected_sensor_sub_index);
}
_voter.print();
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_advertised[i] && (_priority[i] > 0)) {
_calibration[i].PrintStatus();
}
}
}
}; // namespace sensors
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
+140
View File
@@ -0,0 +1,140 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "data_validator/DataValidatorGroup.hpp"
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_preflight_mag.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_magnetometer.h>
namespace sensors
{
class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleMagnetometer();
~VehicleMagnetometer() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
void calcMagInconsistency();
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<sensor_preflight_mag_s> _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)};
uORB::Publication<vehicle_magnetometer_s> _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_mag), 0},
{this, ORB_ID(sensor_mag), 1},
{this, ORB_ID(sensor_mag), 2},
{this, ORB_ID(sensor_mag), 3}
};
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
// Magnetometer interference compensation
enum class MagCompensationType {
Disabled = 0,
Throttle,
Current_inst0,
Current_inst1
};
MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
hrt_abstime _last_publication_timestamp{0};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
DataValidatorGroup _voter{1};
unsigned _last_failover_count{0};
uint64_t _mag_timestamp_sum{0};
matrix::Vector3f _mag_sum{};
int _mag_sum_count{0};
sensor_mag_s _last_data[MAX_SENSOR_COUNT] {};
bool _advertised[MAX_SENSOR_COUNT] {};
float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
uint8_t _priority[MAX_SENSOR_COUNT] {};
int8_t _selected_sensor_sub_index{-1};
bool _armed{false}; /**< arming status of the vehicle */
DEFINE_PARAMETERS(
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
)
};
}; // namespace sensors
src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py
Executable
+272
View File
@@ -0,0 +1,272 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
File: mag_compensation.py
Author: Tanja Baumann
Email: tanja@auterion.com
Github: https://github.com/baumanta
Description:
Computes linear coefficients for mag compensation from thrust and current
Usage:
python mag_compensation.py /path/to/log/logfile.ulg current --instance 1
Remark:
If your logfile does not contain some of the topics, e.g.battery_status/current_a
you will have to comment out the corresponding parts in the script
"""
import matplotlib.pylab as plt
from mpl_toolkits.mplot3d import Axes3D
from pyulog import ULog
from pyulog.px4 import PX4ULog
from pylab import *
import numpy as np
import textwrap as tw
import argparse
#arguments
parser = argparse.ArgumentParser(description='Calculate compensation parameters from ulog')
parser.add_argument('logfile', type=str, nargs='?', default=[],
help='full path to ulog file')
parser.add_argument('type', type=str, nargs='?', choices=['current', 'thrust'], default=[],
help='Power signal used for compensation, supported is "current" or "thrust".')
parser.add_argument('--instance', type=int, nargs='?', default=0,
help='instance of the current or thrust signal to use (0 or 1)')
args = parser.parse_args()
log_name = args.logfile
comp_type = args.type
comp_instance = args.instance
#Load the log data (produced by pyulog)
log = ULog(log_name)
pxlog = PX4ULog(log)
def get_data(topic_name, variable_name, index):
try:
dataset = log.get_dataset(topic_name, index)
return dataset.data[variable_name]
except:
return []
def ms2s_list(time_ms_list):
if len(time_ms_list) > 0:
return 1e-6 * time_ms_list
else:
return time_ms_list
# Select msgs and copy into arrays
armed = get_data('vehicle_status', 'arming_state', 0)
t_armed = ms2s_list(get_data('vehicle_status', 'timestamp', 0))
if comp_type == "thrust":
power = get_data('vehicle_rates_setpoint', 'thrust_body[2]', comp_instance)
power_t = ms2s_list(get_data('vehicle_rates_setpoint', 'timestamp', comp_instance))
comp_type_param = 1
factor = 1
unit = "[G]"
elif comp_type == "current":
power = get_data('battery_status', 'current_a', comp_instance)
power = np.true_divide(power, 1000) #kA
power_t = ms2s_list(get_data('battery_status', 'timestamp', comp_instance))
comp_type_param = 2 + comp_instance
factor = -1
unit = "[G/kA]"
else:
print("unknown compensation type {}. Supported is either 'thrust' or 'current'.".format(comp_type))
sys.exit(1)
if len(power) == 0:
print("could not retrieve power signal from log, zero data points")
sys.exit(1)
mag0X_body = get_data('sensor_mag', 'x', 0)
mag0Y_body = get_data('sensor_mag', 'y', 0)
mag0Z_body = get_data('sensor_mag', 'z', 0)
t_mag0 = ms2s_list(get_data('sensor_mag', 'timestamp', 0))
mag0_ID = get_data('sensor_mag', 'device_id', 0)
mag1X_body = get_data('sensor_mag', 'x', 1)
mag1Y_body = get_data('sensor_mag', 'y', 1)
mag1Z_body = get_data('sensor_mag', 'z', 1)
t_mag1 = ms2s_list(get_data('sensor_mag', 'timestamp', 1))
mag1_ID = get_data('sensor_mag', 'device_id', 1)
mag2X_body = get_data('sensor_mag', 'x', 2)
mag2Y_body = get_data('sensor_mag', 'y', 2)
mag2Z_body = get_data('sensor_mag', 'z', 2)
t_mag2 = ms2s_list(get_data('sensor_mag', 'timestamp', 2))
mag2_ID = get_data('sensor_mag', 'device_id', 2)
mag3X_body = get_data('sensor_mag', 'x', 3)
mag3Y_body = get_data('sensor_mag', 'y', 3)
mag3Z_body = get_data('sensor_mag', 'z', 3)
t_mag3 = ms2s_list(get_data('sensor_mag', 'timestamp', 3))
mag3_ID = get_data('sensor_mag', 'device_id', 3)
magX_body = []
magY_body = []
magZ_body = []
mag_id = []
t_mag = []
if len(mag0X_body) > 0:
magX_body.append(mag0X_body)
magY_body.append(mag0Y_body)
magZ_body.append(mag0Z_body)
t_mag.append(t_mag0)
mag_id.append(mag0_ID[0])
if len(mag1X_body) > 0:
magX_body.append(mag1X_body)
magY_body.append(mag1Y_body)
magZ_body.append(mag1Z_body)
t_mag.append(t_mag1)
mag_id.append(mag1_ID[0])
if len(mag2X_body) > 0:
magX_body.append(mag2X_body)
magY_body.append(mag2Y_body)
magZ_body.append(mag2Z_body)
t_mag.append(t_mag2)
mag_id.append(mag2_ID[0])
if len(mag3X_body) > 0:
magX_body.append(mag3X_body)
magY_body.append(mag3Y_body)
magZ_body.append(mag3Z_body)
t_mag.append(t_mag3)
mag_id.append(mag3_ID[0])
n_mag = len(magX_body)
#log index does not necessarily match mag calibration instance number
calibration_instance = []
instance_found = False
for idx in range(n_mag):
instance_found = False
for j in range(4):
if mag_id[idx] == log.initial_parameters["CAL_MAG{}_ID".format(j)]:
calibration_instance.append(j)
instance_found = True
if not instance_found:
print('Mag {} calibration instance not found, run compass calibration first.'.format(mag_id[idx]))
#get first arming sequence from data
start_time = 0
stop_time = 0
for i in range(len(armed)-1):
if armed[i] == 1 and armed[i+1] == 2:
start_time = t_armed[i+1]
if armed[i] == 2 and armed[i+1] == 1:
stop_time = t_armed[i+1]
break
#cut unarmed sequences from mag data
index_start = 0
index_stop = 0
for idx in range(n_mag):
for i in range(len(t_mag[idx])):
if t_mag[idx][i] > start_time:
index_start = i
break
for i in range(len(t_mag[idx])):
if t_mag[idx][i] > stop_time:
index_stop = i -1
break
t_mag[idx] = t_mag[idx][index_start:index_stop]
magX_body[idx] = magX_body[idx][index_start:index_stop]
magY_body[idx] = magY_body[idx][index_start:index_stop]
magZ_body[idx] = magZ_body[idx][index_start:index_stop]
#resample data
power_resampled = []
for idx in range(n_mag):
power_resampled.append(interp(t_mag[idx], power_t, power))
#fit linear to get coefficients
px = []
py = []
pz = []
for idx in range(n_mag):
px_temp, res_x, _, _, _ = polyfit(power_resampled[idx], magX_body[idx], 1,full = True)
py_temp, res_y, _, _, _ = polyfit(power_resampled[idx], magY_body[idx], 1,full = True)
pz_temp, res_z, _, _, _ = polyfit(power_resampled[idx], magZ_body[idx], 1, full = True)
px.append(px_temp)
py.append(py_temp)
pz.append(pz_temp)
#print to console
for idx in range(n_mag):
print('Mag{} device ID {} (calibration instance {})'.format(idx, mag_id[idx], calibration_instance[idx]))
print('\033[91m \n{}-based compensation: \033[0m'.format(comp_type))
print('\nparam set CAL_MAG_COMP_TYP {}'.format(comp_type_param))
for idx in range(n_mag):
print('\nparam set CAL_MAG{}_XCOMP {:.3f}'.format(calibration_instance[idx], factor * px[idx][0]))
print('param set CAL_MAG{}_YCOMP {:.3f}'.format(calibration_instance[idx], factor * py[idx][0]))
print('param set CAL_MAG{}_ZCOMP {:.3f}'.format(calibration_instance[idx], factor * pz[idx][0]))
#plot data
for idx in range(n_mag):
fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k')
fig.suptitle('Compensation Parameter Fit \n{} \nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold')
plt.subplot(1,3,1)
plt.plot(power_resampled[idx], magX_body[idx], 'yo', power_resampled[idx], px[idx][0]*power_resampled[idx]+px[idx][1], '--k')
plt.xlabel('current [kA]')
plt.ylabel('mag X [G]')
plt.subplot(1,3,2)
plt.plot(power_resampled[idx], magY_body[idx], 'yo', power_resampled[idx], py[idx][0]*power_resampled[idx]+py[idx][1], '--k')
plt.xlabel('current [kA]')
plt.ylabel('mag Y [G]')
plt.subplot(1,3,3)
plt.plot(power_resampled[idx], magZ_body[idx], 'yo', power_resampled[idx], pz[idx][0]*power_resampled[idx]+pz[idx][1], '--k')
plt.xlabel('current [kA]')
plt.ylabel('mag Z [G]')
# display results
plt.figtext(0.24, 0.03, 'CAL_MAG{}_XCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * px[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold')
plt.figtext(0.51, 0.03, 'CAL_MAG{}_YCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * py[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold')
plt.figtext(0.79, 0.03, 'CAL_MAG{}_ZCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * pz[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold')
#compensation comparison plots
for idx in range(n_mag):
fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k')
fig.suptitle('Original Data vs. Compensation \n{}\nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold')
plt.subplot(3,1,1)
original_x, = plt.plot(t_mag[idx], magX_body[idx], label='original')
power_x, = plt.plot(t_mag[idx],magX_body[idx] - px[idx][0] * power_resampled[idx], label='compensated')
plt.legend(handles=[original_x, power_x])
plt.xlabel('Time [s]')
plt.ylabel('Mag X corrected[G]')
plt.subplot(3,1,2)
original_y, = plt.plot(t_mag[idx], magY_body[idx], label='original')
power_y, = plt.plot(t_mag[idx],magY_body[idx] - py[idx][0] * power_resampled[idx], label='compensated')
plt.legend(handles=[original_y, power_y])
plt.xlabel('Time [s]')
plt.ylabel('Mag Y corrected[G]')
plt.subplot(3,1,3)
original_z, = plt.plot(t_mag[idx], magZ_body[idx], label='original')
power_z, = plt.plot(t_mag[idx],magZ_body[idx] - pz[idx][0] * power_resampled[idx], label='compensated')
plt.legend(handles=[original_z, power_z])
plt.xlabel('Time [s]')
plt.ylabel('Mag Z corrected[G]')
plt.show()
src/modules/sensors/voted_sensors_update.cpp
+6 -283
View File
@@ -39,32 +39,20 @@
#include "voted_sensors_update.h"
#include <lib/conversion/rotation.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#define MAG_ROT_VAL_INTERNAL -1
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
using namespace sensors;
using namespace matrix;
using namespace time_literals;
using math::radians;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
ModuleParams(nullptr),
_vehicle_imu_sub(vehicle_imu_sub),
_parameters(parameters),
_hil_enabled(hil_enabled),
_mag_compensator(this)
_hil_enabled(hil_enabled)
{
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
_gyro.voter.set_timeout(500000);
_accel.voter.set_timeout(500000);
@@ -87,26 +75,12 @@ void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(_gyro, GYRO_COUNT_MAX);
initSensorClass(_accel, ACCEL_COUNT_MAX);
initSensorClass(_mag, MAG_COUNT_MAX);
}
void VotedSensorsUpdate::parametersUpdate()
{
updateParams();
/* fine tune board offset */
const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}};
_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
// initialze all mag rotations with the board rotation in case there is no calibration data available
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
_mag_rotation[topic_instance] = _board_rotation;
}
/* set offset parameters to new values */
bool failed = false;
// run through all IMUs
for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) {
uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
@@ -141,159 +115,6 @@ void VotedSensorsUpdate::parametersUpdate()
}
}
}
/* run through all mag sensors
* Because we store the device id in _mag_device_id, we need to get the id via uorb topic since
* the DevHandle method does not work on POSIX.
*/
/* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers,
* and not all uORB instances have to be published yet at the initial call of parametersUpdate()
*/
for (int i = 0; i < MAG_COUNT_MAX; i++) {
_mag.enabled[i] = true;
}
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX
&& topic_instance < _mag.subscription_count; ++topic_instance) {
sensor_mag_s report{};
if (!_mag.subscription[topic_instance].copy(&report)) {
continue;
}
uint32_t topic_device_id = report.device_id;
bool is_external = report.is_external;
_mag_device_id[topic_instance] = topic_device_id;
// find the driver handle that matches the topic_device_id
int fd = -1;
char str[30] {};
for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) {
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
fd = px4_open(str, O_RDWR);
if (fd < 0) {
/* the driver is not running, continue with the next */
continue;
}
uint32_t driver_device_id = (uint32_t)px4_ioctl(fd, DEVIOCGDEVICEID, 0);
if (driver_device_id == topic_device_id) {
break; // we found the matching driver
} else {
px4_close(fd);
}
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < MAG_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id = 0;
failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_MAG%u_EN", i);
int32_t device_enabled = 1;
failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
if (failed) {
continue;
}
/* if the calibration is for this device, apply it */
if ((uint32_t)device_id == _mag_device_id[topic_instance]) {
_mag.enabled[topic_instance] = (device_enabled == 1);
_mag.power_compensation[topic_instance] = {_parameters.mag_comp_paramX[i], _parameters.mag_comp_paramY[i], _parameters.mag_comp_paramZ[i]};
// the mags that were published after the initial parameterUpdate
// would be given the priority even if disabled. Reset it to 0 in this case
if (!_mag.enabled[topic_instance]) {
_mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED;
}
mag_calibration_s mscale{};
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_offset));
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_offset));
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_offset));
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_scale));
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_scale));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_scale));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
int32_t mag_rot = 0;
param_get(param_find(str), &mag_rot);
if (is_external) {
/* check if this mag is still set as internal, otherwise leave untouched */
if (mag_rot < 0) {
/* it was marked as internal, change to external with no rotation */
mag_rot = 0;
param_set_no_notification(param_find(str), &mag_rot);
}
} else {
/* mag is internal - reset param to -1 to indicate internal mag */
if (mag_rot != MAG_ROT_VAL_INTERNAL) {
mag_rot = MAG_ROT_VAL_INTERNAL;
param_set_no_notification(param_find(str), &mag_rot);
}
}
/* now get the mag rotation */
if (mag_rot >= 0) {
// Set external magnetometers to use the parameter value
_mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot);
} else {
// Set internal magnetometers to use the board rotation
_mag_rotation[topic_instance] = _board_rotation;
}
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
config_ok = (px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale) == 0);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
}
}
break;
}
}
px4_close(fd);
}
}
void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
@@ -398,51 +219,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
}
}
void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
{
for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) {
sensor_mag_s mag_report;
if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) {
// First publication with data
if (_mag.priority[uorb_index] == 0) {
_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
}
Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
//throttle-/current-based mag compensation
_mag_compensator.calculate_mag_corrected(vect, _mag.power_compensation[uorb_index]);
vect = _mag_rotation[uorb_index] * vect;
_last_magnetometer[uorb_index].timestamp = mag_report.timestamp;
_last_magnetometer[uorb_index].magnetometer_ga[0] = vect(0);
_last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1);
_last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2);
_mag.voter.put(uorb_index, mag_report.timestamp, _last_magnetometer[uorb_index].magnetometer_ga, mag_report.error_count,
_mag.priority[uorb_index]);
}
}
int best_index;
_mag.voter.get_best(hrt_absolute_time(), &best_index);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
if (best_index >= 0) {
magnetometer = _last_magnetometer[best_index];
_mag.last_best_vote = (uint8_t)best_index;
if (_selection.mag_device_id != _mag_device_id[best_index]) {
_selection_changed = true;
_selection.mag_device_id = _mag_device_id[best_index];
}
}
}
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
{
if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
@@ -494,8 +270,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
if (type == subsystem_info_s::SUBSYSTEM_TYPE_MAG) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
}
_info.timestamp = hrt_absolute_time();
@@ -558,16 +332,11 @@ void VotedSensorsUpdate::printStatus()
PX4_INFO_RAW("\n");
PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote);
_accel.voter.print();
PX4_INFO_RAW("\n");
PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote);
_mag.voter.print();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer)
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
{
imuPoll(raw);
magPoll(magnetometer);
// publish sensor selection if changed
if (_selection_changed) {
@@ -588,8 +357,7 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
}
}
void
VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
{
float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
@@ -638,7 +406,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
}
}
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
{
float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
unsigned check_index = 0; // the number of sensors the primary has been checked against
@@ -686,48 +454,3 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq);
}
}
void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
{
Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector
float mag_angle_diff_max = 0.0f; // the maximum angle difference
unsigned check_index = 0; // the number of sensors the primary has been checked against
// Check each sensor against the primary
for (int i = 0; i < _mag.subscription_count; i++) {
// check that the sensor we are checking against is not the same as the primary
if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
// calculate angle to 3D magnetic field vector of the primary sensor
Vector3f current_mag(_last_magnetometer[i].magnetometer_ga);
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
// complementary filter to not fail/pass on single outliers
_mag_angle_diff[check_index] *= 0.95f;
_mag_angle_diff[check_index] += 0.05f * angle_error;
mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
// increment the check index
check_index++;
}
// check to see if the maximum number of checks has been reached and break
if (check_index >= 2) {
break;
}
}
// get the vector length of the largest difference and write to the combined sensor struct
// will be zero if there is only one magnetometer and hence nothing to compare
preflt.mag_inconsistency_angle = mag_angle_diff_max;
}
void VotedSensorsUpdate::update_mag_comp_armed(bool armed)
{
_mag_compensator.update_armed_flag(armed);
}
void VotedSensorsUpdate::update_mag_comp_power(float power)
{
_mag_compensator.update_power(power);
}
src/modules/sensors/voted_sensors_update.h
+20 -55
View File
@@ -39,35 +39,33 @@
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
#include "data_validator/DataValidator.hpp"
#include "data_validator/DataValidatorGroup.hpp"
#include <drivers/drv_mag.h>
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/mag_compensation/MagCompensation.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
#include "common.h"
namespace sensors
{
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX);
/**
** class VotedSensorsUpdate
*
@@ -80,8 +78,7 @@ public:
* @param parameters parameter values. These do not have to be initialized when constructing this object.
* Only when calling init(), they have to be initialized.
*/
VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
/**
* initialize subscriptions etc.
@@ -105,7 +102,7 @@ public:
/**
* read new sensor data
*/
void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer);
void sensorsPoll(sensor_combined_s &raw);
/**
* set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
@@ -116,34 +113,22 @@ public:
/**
* Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
*/
void calcAccelInconsistency(sensor_preflight_s &preflt);
void calcAccelInconsistency(sensor_preflight_imu_s &preflt);
/**
* Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
*/
void calcGyroInconsistency(sensor_preflight_s &preflt);
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
void calcMagInconsistency(sensor_preflight_s &preflt);
/**
* Update armed flag for mag compensation.
*/
void update_mag_comp_armed(bool armed);
/**
* Update power signal for mag compensation.
* power: either throttle value [0,1] or current measurement in [kA]
*/
void update_mag_comp_power(float power);
void calcGyroInconsistency(sensor_preflight_imu_s &preflt);
private:
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
struct SensorData {
SensorData() = delete;
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
DataValidatorGroup voter{1};
@@ -151,9 +136,8 @@ private:
ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
uint8_t last_best_vote{0}; /**< index of the latest best vote */
uint8_t subscription_count{0};
bool enabled[SENSOR_COUNT_MAX] {true, true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false, false};
matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
bool enabled[SENSOR_COUNT_MAX] {true, true, true};
bool advertised[SENSOR_COUNT_MAX] {false, false, false};
};
void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
@@ -166,14 +150,6 @@ private:
*/
void imuPoll(sensor_combined_s &raw);
/**
* Poll the magnetometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void magPoll(vehicle_magnetometer_s &magnetometer);
/**
* Check & handle failover of a sensor
* @return true if a switch occured (could be for a non-critical reason)
@@ -182,7 +158,6 @@ private:
SensorData _accel{ORB_ID::sensor_accel};
SensorData _gyro{ORB_ID::sensor_gyro};
SensorData _mag{ORB_ID::sensor_mag};
hrt_abstime _last_error_message{0};
orb_advert_t _mavlink_log_pub{nullptr};
@@ -198,26 +173,16 @@ private:
};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */
matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {}; /**< rotation matrix for the orientation that the external mag0 is mounted */
const Parameters &_parameters;
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */
float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
/* Magnetometer interference compensation */
MagCompensator _mag_compensator;
uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */
src/modules/simulator/simulator.cpp
+1 -7
View File
@@ -64,12 +64,6 @@ void Simulator::parameters_update(bool force)
}
}
void Simulator::print_status()
{
PX4_INFO("magnetometer");
_px4_mag.print_status();
}
int Simulator::start(int argc, char *argv[])
{
_instance = new Simulator();
@@ -154,7 +148,7 @@ int simulator_main(int argc, char *argv[])
return 1;
} else {
Simulator::getInstance()->print_status();
PX4_INFO("running");
}
} else {
src/modules/simulator/simulator.h
-2
View File
@@ -127,8 +127,6 @@ public:
bool has_initialized() { return _has_initialized.load(); }
#endif
void print_status();
private:
Simulator() : ModuleParams(nullptr)
{