PX4 Firmware
PX4 Autopilot Software http://px4.io
VehicleAngularVelocity.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 
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::rate_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.gyro_bias};
92  }
93  }
94 }
95 
96 bool
98 {
99  if (_sensor_selection_sub.updated() || force) {
100  sensor_selection_s sensor_selection;
101 
102  if (_sensor_selection_sub.copy(&sensor_selection)) {
103  if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
104  _selected_sensor_device_id = sensor_selection.gyro_device_id;
105  force = true;
106  }
107  }
108  }
109 
110  // check if the selected sensor has updated
111  if (_sensor_correction_sub.updated() || force) {
112 
113  sensor_correction_s corrections{};
114  _sensor_correction_sub.copy(&corrections);
115 
116  // TODO: should be checking device ID
117  if (_selected_sensor == 0) {
118  _offset = Vector3f{corrections.gyro_offset_0};
119  _scale = Vector3f{corrections.gyro_scale_0};
120 
121  } else if (_selected_sensor == 1) {
122  _offset = Vector3f{corrections.gyro_offset_1};
123  _scale = Vector3f{corrections.gyro_scale_1};
124 
125  } else if (_selected_sensor == 2) {
126  _offset = Vector3f{corrections.gyro_offset_2};
127  _scale = Vector3f{corrections.gyro_scale_2};
128 
129  } else {
130  _offset = Vector3f{0.0f, 0.0f, 0.0f};
131  _scale = Vector3f{1.0f, 1.0f, 1.0f};
132  }
133 
134  // update the latest sensor selection
135  if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
136  if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
137  // clear all registered callbacks
138  for (auto &sub : _sensor_control_sub) {
139  sub.unregisterCallback();
140  }
141 
142  for (auto &sub : _sensor_sub) {
143  sub.unregisterCallback();
144  }
145 
146  const int sensor_new = corrections.selected_gyro_instance;
147 
148  // subscribe to sensor_gyro_control if available
149  // currently not all drivers (eg df_*) provide sensor_gyro_control
150  for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
151  sensor_gyro_control_s report{};
152  _sensor_control_sub[i].copy(&report);
153 
154  if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
155  if (_sensor_control_sub[i].registerCallback()) {
156  PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
158 
160 
161  // record selected sensor (sensor_gyro orb index)
162  _selected_sensor = sensor_new;
163 
164  return true;
165  }
166  }
167  }
168 
169  // otherwise fallback to using sensor_gyro (legacy that will be removed)
171 
172  if (_sensor_sub[sensor_new].registerCallback()) {
173  PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
174  _selected_sensor = sensor_new;
175 
176  return true;
177  }
178  }
179  }
180  }
181 
182  return false;
183 }
184 
185 void
187 {
188  // Check if parameters have changed
189  if (_params_sub.updated() || force) {
190  // clear update
191  parameter_update_s param_update;
192  _params_sub.copy(&param_update);
193 
194  updateParams();
195 
196  // get transformation matrix from sensor/board to body frame
197  const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
198 
199  // fine tune the rotation
200  const Dcmf board_rotation_offset(Eulerf(
201  math::radians(_param_sens_board_x_off.get()),
202  math::radians(_param_sens_board_y_off.get()),
203  math::radians(_param_sens_board_z_off.get())));
204 
205  _board_rotation = board_rotation_offset * board_rotation;
206  }
207 }
208 
209 void
211 {
212  // update corrections first to set _selected_sensor
213  bool sensor_select_update = SensorCorrectionsUpdate();
214 
216  // using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
217  if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) {
218  sensor_gyro_control_s sensor_data;
220 
223 
224  // get the sensor data and correct for thermal errors (apply offsets and scale)
225  Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
226 
227  // rotate corrected measurements from sensor to body frame
228  rates = _board_rotation * rates;
229 
230  // correct for in-run bias errors
231  rates -= _bias;
232 
233  vehicle_angular_velocity_s angular_velocity;
234  angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
235  rates.copyTo(angular_velocity.xyz);
236  angular_velocity.timestamp = hrt_absolute_time();
237 
238  _vehicle_angular_velocity_pub.publish(angular_velocity);
239  }
240 
241  } else {
242  // otherwise fallback to using sensor_gyro (legacy that will be removed)
243  if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
244  sensor_gyro_s sensor_data;
245  _sensor_sub[_selected_sensor].copy(&sensor_data);
246 
249 
250  // get the sensor data and correct for thermal errors
251  const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
252 
253  // apply offsets and scale
254  Vector3f rates{(val - _offset).emult(_scale)};
255 
256  // rotate corrected measurements from sensor to body frame
257  rates = _board_rotation * rates;
258 
259  // correct for in-run bias errors
260  rates -= _bias;
261 
262  vehicle_angular_velocity_s angular_velocity;
263  angular_velocity.timestamp_sample = sensor_data.timestamp;
264  rates.copyTo(angular_velocity.xyz);
265  angular_velocity.timestamp = hrt_absolute_time();
266 
267  _vehicle_angular_velocity_pub.publish(angular_velocity);
268  }
269  }
270 }
271 
272 void
274 {
275  PX4_INFO("selected sensor: %d", _selected_sensor);
276 
277  if (_selected_sensor_device_id != 0) {
278  PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
279 
280  } else {
281  PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor);
282  }
283 }
bool copy(void *dst)
Copy the struct.
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub
selected primary sensor subscription
void SensorBiasUpdate(bool force=false)
uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT]
sensor control data subscription
bool updated()
Check if there is a new update.
matrix::Dcmf _board_rotation
rotation matrix for the orientation that the board is mounted
void ParametersUpdate(bool force=false)
uORB::Subscription _sensor_bias_sub
sensor in-run bias correction subscription
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
static constexpr int MAX_SENSOR_COUNT
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Euler< float > Eulerf
Definition: Euler.hpp:156
void zero()
Definition: Matrix.hpp:421
uint64_t timestamp
Definition: sensor_gyro.h:53
bool updated()
Check if there is a new update.
float gyro_bias[3]
Definition: sensor_bias.h:54
bool SensorCorrectionsUpdate(bool force=false)
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
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT]
sensor data subscription
uORB::Subscription _sensor_correction_sub
sensor thermal correction subscription
Definition: bst.cpp:62
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
Definition: rotation.cpp:45
bool copy(void *dst)
Copy the struct.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).