Skip to content

Commit 36c6e36

Browse files
dagardagar
committed
sensors: refactor common corrections and rotation code
1 parent ca96b6b commit 36c6e36

14 files changed

+403
-322
lines changed

‎src/modules/sensors/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,8 @@
3131
#
3232
############################################################################
3333

34+
add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_velocity, imu}
35+
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
3436
add_subdirectory(vehicle_acceleration)
3537
add_subdirectory(vehicle_angular_velocity)
3638
add_subdirectory(vehicle_imu)

‎src/modules/sensors/sensor_corrections/CMakeLists.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
############################################################################
2+
#
3+
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
4+
#
5+
# Redistribution and use in source and binary forms, with or without
6+
# modification, are permitted provided that the following conditions
7+
# are met:
8+
#
9+
# 1. Redistributions of source code must retain the above copyright
10+
# notice, this list of conditions and the following disclaimer.
11+
# 2. Redistributions in binary form must reproduce the above copyright
12+
# notice, this list of conditions and the following disclaimer in
13+
# the documentation and/or other materials provided with the
14+
# distribution.
15+
# 3. Neither the name PX4 nor the names of its contributors may be
16+
# used to endorse or promote products derived from this software
17+
# without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
# POSSIBILITY OF SUCH DAMAGE.
31+
#
32+
############################################################################
33+
34+
px4_add_library(sensor_corrections
35+
SensorCorrections.cpp
36+
SensorCorrections.hpp
37+
)

‎src/modules/sensors/sensor_corrections/SensorCorrections.cpp

+170
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,170 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
#include "SensorCorrections.hpp"
35+
36+
using namespace matrix;
37+
using namespace time_literals;
38+
using math::radians;
39+
40+
namespace sensors
41+
{
42+
43+
SensorCorrections::SensorCorrections(ModuleParams *parent, SensorType type) :
44+
ModuleParams(parent),
45+
_type(type)
46+
{
47+
}
48+
49+
void SensorCorrections::set_device_id(uint32_t device_id)
50+
{
51+
if (_device_id != device_id) {
52+
_device_id = device_id;
53+
SensorCorrectionsUpdate(true);
54+
}
55+
}
56+
57+
const char *SensorCorrections::SensorString() const
58+
{
59+
switch (_type) {
60+
case SensorType::Accelerometer:
61+
return "ACC";
62+
63+
case SensorType::Gyroscope:
64+
return "GYRO";
65+
}
66+
67+
return nullptr;
68+
}
69+
70+
void SensorCorrections::SensorCorrectionsUpdate(bool force)
71+
{
72+
// check if the selected sensor has updated
73+
if (_sensor_correction_sub.updated() || force) {
74+
75+
sensor_correction_s corrections;
76+
77+
if (_sensor_correction_sub.copy(&corrections)) {
78+
79+
// selected sensor has changed, find updated index
80+
if ((_corrections_selected_instance < 0) || force) {
81+
_corrections_selected_instance = -1;
82+
83+
// find sensor_corrections index
84+
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
85+
86+
switch (_type) {
87+
case SensorType::Accelerometer:
88+
if (corrections.accel_device_ids[i] == _device_id) {
89+
_corrections_selected_instance = i;
90+
}
91+
92+
break;
93+
94+
case SensorType::Gyroscope:
95+
if (corrections.gyro_device_ids[i] == _device_id) {
96+
_corrections_selected_instance = i;
97+
}
98+
99+
break;
100+
}
101+
}
102+
}
103+
104+
switch (_type) {
105+
case SensorType::Accelerometer:
106+
switch (_corrections_selected_instance) {
107+
case 0:
108+
_offset = Vector3f{corrections.accel_offset_0};
109+
_scale = Vector3f{corrections.accel_scale_0};
110+
return;
111+
case 1:
112+
_offset = Vector3f{corrections.accel_offset_1};
113+
_scale = Vector3f{corrections.accel_scale_1};
114+
return;
115+
case 2:
116+
_offset = Vector3f{corrections.accel_offset_2};
117+
_scale = Vector3f{corrections.accel_scale_2};
118+
return;
119+
}
120+
121+
break;
122+
123+
case SensorType::Gyroscope:
124+
switch (_corrections_selected_instance) {
125+
case 0:
126+
_offset = Vector3f{corrections.gyro_offset_0};
127+
_scale = Vector3f{corrections.gyro_scale_0};
128+
return;
129+
case 1:
130+
_offset = Vector3f{corrections.gyro_offset_1};
131+
_scale = Vector3f{corrections.gyro_scale_1};
132+
return;
133+
case 2:
134+
_offset = Vector3f{corrections.gyro_offset_2};
135+
_scale = Vector3f{corrections.gyro_scale_2};
136+
return;
137+
}
138+
139+
break;
140+
}
141+
}
142+
}
143+
}
144+
145+
void SensorCorrections::ParametersUpdate()
146+
{
147+
// fine tune the rotation
148+
const Dcmf board_rotation_offset(Eulerf(
149+
radians(_param_sens_board_x_off.get()),
150+
radians(_param_sens_board_y_off.get()),
151+
radians(_param_sens_board_z_off.get())));
152+
153+
// get transformation matrix from sensor/board to body frame
154+
_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
155+
}
156+
157+
void SensorCorrections::PrintStatus()
158+
{
159+
if (_offset.norm() > 0.f) {
160+
PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1),
161+
(double)_offset(2));
162+
}
163+
164+
if (fabsf(_scale.norm_squared() - 3.f) > FLT_EPSILON) {
165+
PX4_INFO("%s %d scale: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_scale(0), (double)_scale(1),
166+
(double)_scale(2));
167+
}
168+
}
169+
170+
} // namespace sensors

‎src/modules/sensors/sensor_corrections/SensorCorrections.hpp

+99
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
#pragma once
35+
36+
#include <lib/conversion/rotation.h>
37+
#include <lib/matrix/matrix/math.hpp>
38+
#include <px4_platform_common/px4_config.h>
39+
#include <px4_platform_common/log.h>
40+
#include <px4_platform_common/module_params.h>
41+
#include <uORB/Subscription.hpp>
42+
#include <uORB/topics/sensor_correction.h>
43+
44+
namespace sensors
45+
{
46+
47+
class SensorCorrections : public ModuleParams
48+
{
49+
public:
50+
51+
enum class SensorType : uint8_t {
52+
Accelerometer,
53+
Gyroscope,
54+
};
55+
56+
SensorCorrections() = delete;
57+
SensorCorrections(ModuleParams *parent, SensorType type);
58+
~SensorCorrections() override = default;
59+
60+
void PrintStatus();
61+
62+
void set_device_id(uint32_t device_id);
63+
uint32_t get_device_id() const { return _device_id; }
64+
65+
// apply offsets and scale
66+
// rotate corrected measurements from sensor to body frame
67+
matrix::Vector3f Correct(const matrix::Vector3f &data) const { return _board_rotation * matrix::Vector3f{(data - _offset).emult(_scale)}; }
68+
69+
void ParametersUpdate();
70+
void SensorCorrectionsUpdate(bool force = false);
71+
72+
private:
73+
74+
static constexpr int MAX_SENSOR_COUNT = 3;
75+
76+
const char *SensorString() const;
77+
78+
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
79+
80+
matrix::Dcmf _board_rotation;
81+
82+
matrix::Vector3f _offset{0.f, 0.f, 0.f};
83+
matrix::Vector3f _scale{1.f, 1.f, 1.f};
84+
85+
uint32_t _device_id{0};
86+
int8_t _corrections_selected_instance{-1};
87+
88+
const SensorType _type;
89+
90+
DEFINE_PARAMETERS(
91+
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
92+
93+
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
94+
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
95+
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
96+
)
97+
};
98+
99+
} // namespace sensors

‎src/modules/sensors/sensors.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -567,11 +567,15 @@ int Sensors::print_status()
567567
PX4_INFO("Airspeed status:");
568568
_airspeed_validator.print();
569569

570+
PX4_INFO_RAW("\n");
570571
_vehicle_acceleration.PrintStatus();
572+
573+
PX4_INFO_RAW("\n");
571574
_vehicle_angular_velocity.PrintStatus();
572575

573576
for (auto &i : _vehicle_imu_list) {
574577
if (i != nullptr) {
578+
PX4_INFO_RAW("\n");
575579
i->PrintStatus();
576580
}
577581
}

‎src/modules/sensors/vehicle_acceleration/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -35,4 +35,4 @@ px4_add_library(vehicle_acceleration
3535
VehicleAcceleration.cpp
3636
VehicleAcceleration.hpp
3737
)
38-
target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)
38+
target_link_libraries(vehicle_acceleration PRIVATE sensor_corrections px4_work_queue)

0 commit comments

Comments
 (0)