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

simple gyro auto calibration module

This commit is contained in:
Daniel Agar 2021-03-06 13:14:57 -05:00 committed by Lorenz Meier
parent 01c9a4f24d
commit 2257c3767e
70 changed files with 682 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@ -1009,6 +1009,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dmesg"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status; param show CAL_GYRO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"'
@ -1071,6 +1072,7 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status; param show CAL_GYRO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"'
// stop logger

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -516,6 +516,11 @@ else
gyro_fft start
fi
if param compare -s IMU_GYRO_CAL_EN 1
then
gyro_calibration start
fi
#
# Optional board supplied extras: rc.board_extras
#

1
boards/airmind/mindpx-v2/default.cmake
View File

@ -61,6 +61,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/atlflight/eagle/default.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/atlflight/eagle/qurt.cmake
View File

@ -58,6 +58,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/atlflight/excelsior/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/atlflight/excelsior/qurt.cmake
View File

@ -58,6 +58,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/av/x-v1/default.cmake
View File

@ -61,6 +61,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/beaglebone/blue/default.cmake
View File

@ -43,6 +43,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/cuav/nora/default.cmake
View File

@ -65,6 +65,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/cuav/x7pro/default.cmake
View File

@ -68,6 +68,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/cubepilot/cubeorange/console.cmake
View File

@ -68,6 +68,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
landing_target_estimator

1
boards/cubepilot/cubeorange/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/cubepilot/cubeyellow/console.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/cubepilot/cubeyellow/default.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/emlid/navio2/default.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/holybro/durandal-v1/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/holybro/kakutef7/default.cmake
View File

@ -38,6 +38,7 @@ px4_add_board(
#ekf2
events
flight_mode_manager
gyro_calibration
#gyro_fft
land_detector
load_mon

1
boards/holybro/pix32v5/default.cmake
View File

@ -70,6 +70,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/modalai/fc-v1/default.cmake
View File

@ -60,6 +60,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/ctrl-zero-f7-oem/default.cmake
View File

@ -64,6 +64,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -64,6 +64,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/ctrl-zero-h7-oem/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/ctrl-zero-h7/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/pixracerpro/default.cmake
View File

@ -66,6 +66,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/x21-777/default.cmake
View File

@ -65,6 +65,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/mro/x21/default.cmake
View File

@ -62,6 +62,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmuk66-e/default.cmake
View File

@ -62,6 +62,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmuk66-e/socketcan.cmake
View File

@ -61,6 +61,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmuk66-v3/default.cmake
View File

@ -62,6 +62,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmuk66-v3/rtps.cmake
View File

@ -61,6 +61,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmuk66-v3/socketcan.cmake
View File

@ -61,6 +61,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
ekf2
events
flight_mode_manager
#gyro_calibration
#gyro_fft
land_detector
landing_target_estimator

1
boards/omnibus/f4sd/default.cmake
View File

@ -50,6 +50,7 @@ px4_add_board(
flight_mode_manager
#fw_att_control
#fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
#landing_target_estimator

1
boards/px4/fmu-v2/default.cmake
View File

@ -76,6 +76,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
#landing_target_estimator

1
boards/px4/fmu-v2/fixedwing.cmake
View File

@ -52,6 +52,7 @@ px4_add_board(
#events
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
load_mon

1
boards/px4/fmu-v2/multicopter.cmake
View File

@ -43,6 +43,7 @@ px4_add_board(
dataman
ekf2
flight_mode_manager
gyro_calibration
#gyro_fft
#events
land_detector

1
boards/px4/fmu-v3/default.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v4/default.cmake
View File

@ -70,6 +70,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v4/uavcanv1.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v4pro/default.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v5/ctrlalloc.cmake
View File

@ -73,6 +73,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v5/debug.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
#landing_target_estimator

1
boards/px4/fmu-v5/default.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -53,6 +53,7 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
load_mon

1
boards/px4/fmu-v5/optimized.cmake
View File

@ -64,6 +64,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
#landing_target_estimator

1
boards/px4/fmu-v5/rtps.cmake
View File

@ -68,6 +68,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
#landing_target_estimator

2
boards/px4/fmu-v5/uavcanv0periph.cmake
View File

@ -73,6 +73,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
#gyro_fft
land_detector
landing_target_estimator
@ -129,7 +130,6 @@ px4_add_board(
#fake_gyro
#fake_magnetometer
#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
#gyro_fft
#hello
#hwtest # Hardware test
#matlab_csv_serial

2
boards/px4/fmu-v5/uavcanv1.cmake
View File

@ -72,6 +72,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator
@ -129,7 +130,6 @@ px4_add_board(
#fake_gyro
#fake_magnetometer
#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
#gyro_fft
#hello
#hwtest # Hardware test
#matlab_csv_serial

1
boards/px4/fmu-v5x/base_phy_DP83848C.cmake
View File

@ -67,6 +67,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v5x/default.cmake
View File

@ -71,6 +71,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v6u/default.cmake
View File

@ -65,6 +65,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/fmu-v6x/default.cmake
View File

@ -69,6 +69,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/raspberrypi/default.cmake
View File

@ -42,6 +42,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/sitl/ctrlalloc.cmake
View File

@ -37,6 +37,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/sitl/default.cmake
View File

@ -36,6 +36,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/sitl/nolockstep.cmake
View File

@ -35,6 +35,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/sitl/rtps.cmake
View File

@ -35,6 +35,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/px4/sitl/test.cmake
View File

@ -35,6 +35,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/scumaker/pilotpi/arm64.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

1
boards/scumaker/pilotpi/default.cmake
View File

@ -44,6 +44,7 @@ px4_add_board(
flight_mode_manager
fw_att_control
fw_pos_control_l1
gyro_calibration
gyro_fft
land_detector
landing_target_estimator

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -88,7 +88,7 @@ static constexpr wq_config_t UART7{"wq:UART7", 1400, -28};
static constexpr wq_config_t UART8{"wq:UART8", 1400, -29};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -30};
static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};
static constexpr wq_config_t test1{"wq:test1", 2000, 0};
static constexpr wq_config_t test2{"wq:test2", 2000, 0};

87
src/lib/mathlib/math/WelfordMean.hpp Normal file
View File

@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2021 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 WelfordMean.hpp
*
* Welford's online algorithm for computing mean and variance.
*/
#pragma once
namespace math
{
template<typename T>
class WelfordMean
{
public:
// For a new value, compute the new count, new mean, the new M2.
void update(const T &new_value)
{
if (_count == 0) {
_mean = new_value;
}
_count++;
// mean accumulates the mean of the entire dataset
const T delta{new_value - _mean};
_mean += delta / _count;
// M2 aggregates the squared distance from the mean
// count aggregates the number of samples seen so far
_M2 += delta.emult(new_value - _mean);
}
bool valid() const { return _count > 2; }
unsigned count() const { return _count; }
void reset()
{
_count = 0;
_mean = {};
_M2 = {};
}
// Retrieve the mean, variance and sample variance
T mean() const { return _mean; }
T variance() const { return _M2 / _count; }
T sample_variance() const { return _M2 / (_count - 1); }
private:
T _mean{};
T _M2{};
unsigned _count{0};
};
} // namespace math

1
src/lib/sensor_calibration/Gyroscope.hpp
View File

@ -73,6 +73,7 @@ public:
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame

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

@ -57,7 +57,6 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
static constexpr char sensor_name[] {"gyro"};
@ -83,9 +82,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
static constexpr unsigned CALIBRATION_COUNT = 250;
unsigned poll_errcount = 0;
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{};
uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
@ -115,39 +111,16 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
sensor_gyro_s gyro_report;
while (gyro_sub[gyro_index].update(&gyro_report)) {
// fetch optional thermal offset corrections in sensor frame
const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
// fetch optional thermal offset corrections in sensor/board frame
Vector3f offset{0, 0, 0};
sensor_correction_sub.update(&sensor_correction);
if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) {
for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.gyro_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.gyro_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.gyro_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.gyro_offset_3};
break;
}
}
}
}
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
calibration_counter[gyro_index]++;
if (gyro_index == 0) {
worker_data.filter[0].insert(gyro_report.x - offset(0));
worker_data.filter[1].insert(gyro_report.y - offset(1));
worker_data.filter[2].insert(gyro_report.z - offset(2));
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
worker_data.filter[2].insert(gyro_report.z - thermal_offset(2));
}
}

43
src/modules/gyro_calibration/CMakeLists.txt Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2021 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_module(
MODULE modules__gyro_calibration
MAIN gyro_calibration
COMPILE_FLAGS
SRCS
GyroCalibration.cpp
GyroCalibration.hpp
DEPENDS
px4_work_queue
)

328
src/modules/gyro_calibration/GyroCalibration.cpp Normal file
View File

@ -0,0 +1,328 @@
/****************************************************************************
*
* Copyright (c) 2021 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 "GyroCalibration.hpp"
#include <lib/ecl/geo/geo.h>
using namespace time_literals;
using matrix::Vector3f;
GyroCalibration::GyroCalibration() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
}
GyroCalibration::~GyroCalibration()
{
perf_free(_loop_interval_perf);
perf_free(_calibration_updated_perf);
}
bool GyroCalibration::init()
{
ScheduleOnInterval(INTERVAL_US);
return true;
}
void GyroCalibration::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
return;
}
perf_count(_loop_interval_perf);
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.copy(&vehicle_status)) {
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
if (armed != _armed) {
if (!_armed && armed) {
// disarmed -> armed: run at minimal rate until disarmed
ScheduleOnInterval(10_s);
} else if (_armed && !armed) {
// armed -> disarmed: start running again
ScheduleOnInterval(INTERVAL_US);
}
_armed = armed;
Reset();
}
}
}
if (_armed) {
// do nothing if armed
return;
}
if (_vehicle_status_flags_sub.updated()) {
vehicle_status_flags_s vehicle_status_flags;
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
if (_system_calibrating != vehicle_status_flags.condition_calibration_enabled) {
Reset();
_system_calibrating = vehicle_status_flags.condition_calibration_enabled;
}
}
}
if (_system_calibrating) {
// do nothing if system is calibrating
return;
}
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
if (_parameter_update_sub.copy(&param_update)) {
// minimize updates immediately following parameter changes
_last_calibration_update = param_update.timestamp;
}
for (auto &cal : _gyro_calibration) {
cal.ParametersUpdate();
}
}
// collect raw data from all available gyroscopes (sensor_gyro)
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
sensor_gyro_s sensor_gyro;
while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) {
if (PX4_ISFINITE(sensor_gyro.temperature)) {
if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) {
PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro],
(double)sensor_gyro.temperature);
_temperature[gyro] = sensor_gyro.temperature;
// reset all on any temperature change
Reset();
}
} else {
_temperature[gyro] = NAN;
}
if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) {
const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()};
_gyro_mean[gyro].update(val);
} else {
// setting device id, reset all
_gyro_calibration[gyro].set_device_id(sensor_gyro.device_id);
Reset();
}
}
}
// check all accelerometers for possible movement
for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) {
sensor_accel_s sensor_accel;
if (_sensor_accel_subs[accel].update(&sensor_accel)) {
const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z};
if ((acceleration - _acceleration[accel]).longerThan(0.5f)) {
// reset all on any change
PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length());
_acceleration[accel] = acceleration;
Reset();
return;
} else if (acceleration.longerThan(CONSTANTS_ONE_G * 1.3f)) {
Reset();
return;
}
}
}
// check if sufficient data has been gathered to update calibration
bool sufficient_samples = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
// periodically check variance
if ((_gyro_mean[gyro].count() % 100 == 0)) {
PX4_DEBUG("gyro %d (%d) variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(),
(double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
(double)_gyro_mean[gyro].variance().length());
if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
// reset all
Reset();
return;
}
}
if (_gyro_mean[gyro].count() > 3000) {
sufficient_samples = true;
} else {
sufficient_samples = false;
return;
}
}
}
// update calibrations for all available gyros
if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) {
bool calibration_updated = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
_gyro_calibration[gyro].set_calibration_index(gyro);
const Vector3f old_offset{_gyro_calibration[gyro].offset()};
if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
calibration_updated = true;
PX4_INFO("gyro %d (%d) updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C",
gyro, _gyro_calibration[gyro].device_id(),
(double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
(double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
(double)_temperature[gyro]);
perf_count(_calibration_updated_perf);
}
}
}
// save all calibrations
if (calibration_updated) {
bool param_save = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
if (_gyro_calibration[gyro].ParametersSave()) {
param_save = true;
}
}
}
if (param_save) {
param_notify_changes();
_last_calibration_update = hrt_absolute_time();
}
}
Reset();
}
}
int GyroCalibration::task_spawn(int argc, char *argv[])
{
GyroCalibration *instance = new GyroCalibration();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int GyroCalibration::print_status()
{
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
PX4_INFO_RAW("gyro %d (%d), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f°C (count %d)\n",
gyro, _gyro_calibration[gyro].device_id(),
(double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
(double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
(double)_temperature[gyro], _gyro_mean[gyro].count());
}
}
perf_print_counter(_loop_interval_perf);
perf_print_counter(_calibration_updated_perf);
return 0;
}
int GyroCalibration::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int GyroCalibration::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Simple online gyroscope calibration.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("gyro_calibration", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int gyro_calibration_main(int argc, char *argv[])
{
return GyroCalibration::main(argc, argv);
}

108
src/modules/gyro_calibration/GyroCalibration.hpp Normal file
View File

@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (c) 2021 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 <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/WelfordMean.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
using namespace time_literals;
class GyroCalibration : public ModuleBase<GyroCalibration>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
GyroCalibration();
~GyroCalibration() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
static constexpr hrt_abstime INTERVAL_US = 20000_us;
static constexpr int MAX_SENSORS = 4;
void Run() override;
void Reset()
{
for (auto &m : _gyro_mean) {
m.reset();
}
}
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status};
uORB::Subscription _vehicle_status_flags_sub{ORB_ID::vehicle_status_flags};
uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
float _temperature[MAX_SENSORS] {};
matrix::Vector3f _acceleration[MAX_SENSORS] {};
hrt_abstime _last_calibration_update{0};
bool _armed{false};
bool _system_calibrating{false};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
perf_counter_t _calibration_updated_perf{perf_alloc(PC_COUNT, MODULE_NAME": calibration updated")};
};

41
src/modules/gyro_calibration/parameters.c Normal file
View File

@ -0,0 +1,41 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
/**
* IMU gyro auto calibration enable.
*
* @boolean
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1);