PX4 Firmware
PX4 Autopilot Software http://px4.io
VehicleAcceleration.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 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 "VehicleAcceleration.hpp"
35 
36 #include <px4_platform_common/log.h>
37 
38 using namespace matrix;
39 using namespace time_literals;
40 
42  ModuleParams(nullptr),
43  WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
44 {
45 }
46 
48 {
49  Stop();
50 }
51 
52 bool
54 {
55  // initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
56  _scale = Vector3f{1.0f, 1.0f, 1.0f};
57  _offset.zero();
58  _bias.zero();
59 
60  // force initial updates
61  ParametersUpdate(true);
62  SensorBiasUpdate(true);
63 
64  // needed to change the active sensor if the primary stops updating
66 
67  return SensorCorrectionsUpdate(true);
68 }
69 
70 void
72 {
73  Deinit();
74 
75  // clear all registered callbacks
76  for (auto &sub : _sensor_sub) {
77  sub.unregisterCallback();
78  }
79 
81 }
82 
83 void
85 {
86  if (_sensor_bias_sub.updated() || force) {
87  sensor_bias_s bias;
88 
89  if (_sensor_bias_sub.copy(&bias)) {
90  // TODO: should be checking device ID
91  _bias = Vector3f{bias.accel_bias};
92  }
93  }
94 }
95 
96 bool
98 {
99  // check if the selected sensor has updated
100  if (_sensor_correction_sub.updated() || force) {
101 
102  sensor_correction_s corrections{};
103  _sensor_correction_sub.copy(&corrections);
104 
105  // TODO: should be checking device ID
106  if (_selected_sensor == 0) {
107  _offset = Vector3f{corrections.accel_offset_0};
108  _scale = Vector3f{corrections.accel_scale_0};
109 
110  } else if (_selected_sensor == 1) {
111  _offset = Vector3f{corrections.accel_offset_1};
112  _scale = Vector3f{corrections.accel_scale_1};
113 
114  } else if (_selected_sensor == 2) {
115  _offset = Vector3f{corrections.accel_offset_2};
116  _scale = Vector3f{corrections.accel_scale_2};
117 
118  } else {
119  _offset = Vector3f{0.0f, 0.0f, 0.0f};
120  _scale = Vector3f{1.0f, 1.0f, 1.0f};
121  }
122 
123  // update the latest sensor selection
124  if ((_selected_sensor != corrections.selected_accel_instance) || force) {
125  if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
126  // clear all registered callbacks
127  for (auto &sub : _sensor_sub) {
128  sub.unregisterCallback();
129  }
130 
131  const int sensor_new = corrections.selected_accel_instance;
132 
133  if (_sensor_sub[sensor_new].registerCallback()) {
134  PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
135  _selected_sensor = sensor_new;
136 
137  return true;
138  }
139  }
140  }
141  }
142 
143  return false;
144 }
145 
146 void
148 {
149  // Check if parameters have changed
150  if (_params_sub.updated() || force) {
151  // clear update
152  parameter_update_s param_update;
153  _params_sub.copy(&param_update);
154 
155  updateParams();
156 
157  // get transformation matrix from sensor/board to body frame
158  const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
159 
160  // fine tune the rotation
161  const Dcmf board_rotation_offset(Eulerf(
162  math::radians(_param_sens_board_x_off.get()),
163  math::radians(_param_sens_board_y_off.get()),
164  math::radians(_param_sens_board_z_off.get())));
165 
166  _board_rotation = board_rotation_offset * board_rotation;
167  }
168 }
169 
170 void
172 {
173  // update corrections first to set _selected_sensor
174  bool sensor_select_update = SensorCorrectionsUpdate();
175 
176  if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
177  sensor_accel_s sensor_data;
178  _sensor_sub[_selected_sensor].copy(&sensor_data);
179 
182 
183  // get the sensor data and correct for thermal errors
184  const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
185 
186  // apply offsets and scale
187  Vector3f accel{(val - _offset).emult(_scale)};
188 
189  // rotate corrected measurements from sensor to body frame
190  accel = _board_rotation * accel;
191 
192  // correct for in-run bias errors
193  accel -= _bias;
194 
196  out.timestamp_sample = sensor_data.timestamp;
197  accel.copyTo(out.xyz);
198  out.timestamp = hrt_absolute_time();
199 
200  _vehicle_acceleration_pub.publish(out);
201  }
202 }
203 
204 void
206 {
207  PX4_INFO("selected sensor: %d", _selected_sensor);
208 }
bool copy(void *dst)
Copy the struct.
uint64_t timestamp
Definition: sensor_accel.h:53
void ParametersUpdate(bool force=false)
bool SensorCorrectionsUpdate(bool force=false)
uORB::Subscription _sensor_correction_sub
sensor thermal correction subscription
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT]
sensor data subscription
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uORB::Subscription _sensor_bias_sub
sensor in-run bias correction subscription
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Euler< float > Eulerf
Definition: Euler.hpp:156
void zero()
Definition: Matrix.hpp:421
bool updated()
Check if there is a new update.
matrix::Dcmf _board_rotation
rotation matrix for the orientation that the board is mounted
float accel_bias[3]
Definition: sensor_bias.h:55
Definition: bst.cpp:62
void SensorBiasUpdate(bool force=false)
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
Definition: rotation.cpp:45
static constexpr int MAX_SENSOR_COUNT
bool copy(void *dst)
Copy the struct.
DEFINE_PARAMETERS((ParamInt< px4::params::SENS_BOARD_ROT >) _param_sens_board_rot,(ParamFloat< px4::params::SENS_BOARD_X_OFF >) _param_sens_board_x_off,(ParamFloat< px4::params::SENS_BOARD_Y_OFF >) _param_sens_board_y_off,(ParamFloat< px4::params::SENS_BOARD_Z_OFF >) _param_sens_board_z_off) uORB uORB::Subscription _params_sub
parameter updates subscription
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
matrix::Vector3f _offset
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub
selected primary sensor subscription