PX4 Firmware
PX4 Autopilot Software http://px4.io
voted_sensors_update.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 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 /**
37  * @file voted_sensors_update.h
38  *
39  * @author Beat Kueng <beat-kueng@gmx.net>
40  */
41 
42 #include "parameters.h"
43 
44 #include <drivers/drv_accel.h>
45 #include <drivers/drv_gyro.h>
46 #include <drivers/drv_mag.h>
47 #include <drivers/drv_baro.h>
48 #include <drivers/drv_hrt.h>
49 
50 #include <mathlib/mathlib.h>
51 
54 
55 #include <uORB/Publication.hpp>
64 
65 #include <DevMgr.hpp>
66 
68 #include "common.h"
69 
70 namespace sensors
71 {
72 
73 /**
74  ** class VotedSensorsUpdate
75  *
76  * Handling of sensor updates with voting
77  */
79 {
80 public:
81  /**
82  * @param parameters parameter values. These do not have to be initialized when constructing this object.
83  * Only when calling init(), they have to be initialized.
84  */
85  VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled);
86 
87  /**
88  * initialize subscriptions etc.
89  * @return 0 on success, <0 otherwise
90  */
91  int init(sensor_combined_s &raw);
92 
93  /**
94  * This tries to find new sensor instances. This is called from init(), then it can be called periodically.
95  */
96  void initializeSensors();
97 
98  /**
99  * deinitialize the object (we cannot use the destructor because it is called on the wrong thread)
100  */
101  void deinit();
102 
103  void printStatus();
104 
105  /**
106  * call this whenever parameters got updated. Make sure to have initializeSensors() called at least
107  * once before calling this.
108  */
109  void parametersUpdate();
110 
111  /**
112  * read new sensor data
113  */
114  void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer);
115 
116  /**
117  * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
118  * so that the data can be published.
119  */
121 
122  /**
123  * check if a failover event occured. if so, report it.
124  */
125  void checkFailover();
126 
127  int numGyros() const { return _gyro.subscription_count; }
128 
129  int gyroFd(int idx) const { return _gyro.subscription[idx]; }
130 
132 
133  /**
134  * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
135  */
137 
138  /**
139  * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
140  */
142 
143  /**
144  * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
145  */
147 
148 private:
149 
150  struct SensorData {
152  : last_best_vote(0),
154  voter(1),
156  {
157  for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
158  enabled[i] = true;
159  subscription[i] = -1;
160  priority[i] = 0;
161  }
162  }
163 
165 
166  int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
167  uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
168  uint8_t last_best_vote; /**< index of the latest best vote */
171  unsigned int last_failover_count;
172  };
173 
174  void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max);
175 
176  /**
177  * Poll the accelerometer for updated data.
178  *
179  * @param raw Combined sensor data structure into which
180  * data should be returned.
181  */
182  void accelPoll(sensor_combined_s &raw);
183 
184  /**
185  * Poll the gyro for updated data.
186  *
187  * @param raw Combined sensor data structure into which
188  * data should be returned.
189  */
190  void gyroPoll(sensor_combined_s &raw);
191 
192  /**
193  * Poll the magnetometer for updated data.
194  *
195  * @param raw Combined sensor data structure into which
196  * data should be returned.
197  */
198  void magPoll(vehicle_magnetometer_s &magnetometer);
199 
200  /**
201  * Poll the barometer for updated data.
202  *
203  * @param raw Combined sensor data structure into which
204  * data should be returned.
205  */
206  void baroPoll(vehicle_air_data_s &airdata);
207 
208  /**
209  * Check & handle failover of a sensor
210  * @return true if a switch occured (could be for a non-critical reason)
211  */
212  bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type);
213 
214  /**
215  * Apply a gyro calibration.
216  *
217  * @param h: reference to the DevHandle in use
218  * @param gscale: the calibration data.
219  * @param device: the device id of the sensor.
220  * @return: true if config is ok
221  */
222  bool applyGyroCalibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id);
223 
224  /**
225  * Apply a accel calibration.
226  *
227  * @param h: reference to the DevHandle in use
228  * @param ascale: the calibration data.
229  * @param device: the device id of the sensor.
230  * @return: true if config is ok
231  */
232  bool applyAccelCalibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal,
233  const int device_id);
234 
235  /**
236  * Apply a mag calibration.
237  *
238  * @param h: reference to the DevHandle in use
239  * @param gscale: the calibration data.
240  * @param device: the device id of the sensor.
241  * @return: true if config is ok
242  */
243  bool applyMagCalibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id);
244 
249 
251 
252  uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */
253  uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
254 
255  sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
256  vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
257  vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
258 
259  matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */
260  matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {}; /**< rotation matrix for the orientation that the external mag0 is mounted */
261 
263  const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
264 
265  bool _corrections_changed{false};
266  bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */
267 
268  float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
269  float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
270  float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */
271 
272  uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
273  uint32_t _baro_device_id[SENSOR_COUNT_MAX] {}; /**< baro driver device id for each uorb instance */
274  uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
275  uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */
276 
277  uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */
278 
279  sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */
280  sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
281  subsystem_info_s _info {}; /**< subsystem info publication */
282 
283  TemperatureCompensation _temperature_compensation{}; /**< sensor thermal compensation */
284 
285  uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
286 };
287 
288 } /* namespace sensors */
uint32_t _baro_device_id[SENSOR_COUNT_MAX]
baro driver device id for each uorb instance
class VotedSensorsUpdate
sensor_selection_s _selection
struct containing the sensor selection to be published to the uORB
Accelerometer driver interface.
bool applyMagCalibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
Apply a mag calibration.
vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX]
latest sensor data from all sensors instances
Gyroscope driver interface.
void magPoll(vehicle_magnetometer_s &magnetometer)
Poll the magnetometer for updated data.
VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer)
read new sensor data
gyro scaling factors; Vout = (Vin * Vscale) + Voffset
Definition: drv_gyro.h:54
uORB::Publication< sensor_correction_s > _sensor_correction_pub
handle to the sensor correction uORB topic
void setRelativeTimestamps(sensor_combined_s &raw)
set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, so that the data can be published.
sensor_correction_s _corrections
struct containing the sensor corrections to be published to the uORB
void baroPoll(vehicle_air_data_s &airdata)
Poll the barometer for updated data.
vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX]
latest sensor data from all sensors instances
mag scaling factors; Vout = (Vin * Vscale) + Voffset
Definition: drv_mag.h:56
void initializeSensors()
This tries to find new sensor instances.
static const char * sensor_name
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
int init(sensor_combined_s &raw)
initialize subscriptions etc.
void calcMagInconsistency(sensor_preflight_s &preflt)
Calculates the magnitude in Gauss of the largest difference between the primary and any other magneto...
accel scaling factors; Vout = Vscale * (Vin + Voffset)
Definition: drv_accel.h:54
uint32_t _gyro_device_id[SENSOR_COUNT_MAX]
gyro driver device id for each uorb instance
const bool _hil_enabled
is hardware-in-the-loop mode enabled?
class TemperatureCompensation Applies temperature compensation to sensor data.
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]
latest sensor data from all sensors instances
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
constexpr uint8_t ACCEL_COUNT_MAX
Definition: common.h:48
constexpr uint8_t MAG_COUNT_MAX
Definition: common.h:46
bool _selection_changed
true when a sensor selection has changed and not been published
Sensor correction methods.
uint8_t priority[SENSOR_COUNT_MAX]
sensor priority
float _gyro_diff[3][2]
filtered gyro differences between IMU uinits (rad/s)
int subscription[SENSOR_COUNT_MAX]
raw sensor data subscription
A data validation group to identify anomalies in data streams.
uint32_t _accel_device_id[SENSOR_COUNT_MAX]
accel driver device id for each uorb instance
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]
latest full timestamp
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max)
constexpr uint8_t SENSOR_COUNT_MAX
Definition: common.h:51
Definition: common.h:43
Object metadata.
Definition: uORB.h:50
void calcAccelInconsistency(sensor_preflight_s &preflt)
Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel s...
float _mag_angle_diff[2]
filtered mag angle differences between sensor instances (Ga)
TemperatureCompensation _temperature_compensation
sensor thermal compensation
matrix::Dcmf _mag_rotation[MAG_COUNT_MAX]
rotation matrix for the orientation that the external mag0 is mounted
bool applyAccelCalibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
Apply a accel calibration.
uint32_t _mag_device_id[SENSOR_COUNT_MAX]
mag driver device id for each uorb instance
void parametersUpdate()
call this whenever parameters got updated.
matrix::Dcmf _board_rotation
rotation matrix for the orientation that the board is mounted
uORB::Publication< sensor_selection_s > _sensor_selection_pub
handle to the sensor selection uORB topic
void accelPoll(sensor_combined_s &raw)
Poll the accelerometer for updated data.
uORB::PublicationQueued< subsystem_info_s > _info_pub
A data validation class to identify anomalies in data streams.
Barometric pressure sensor driver interface.
subsystem_info_s _info
subsystem info publication
void deinit()
deinitialize the object (we cannot use the destructor because it is called on the wrong thread) ...
void checkFailover()
check if a failover event occured.
void gyroPoll(sensor_combined_s &raw)
Poll the gyro for updated data.
float _accel_diff[3][2]
filtered accel differences between IMU units (m/s/s)
void calcGyroInconsistency(sensor_preflight_s &preflt)
Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro se...
uint8_t last_best_vote
index of the latest best vote
bool applyGyroCalibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
Apply a gyro calibration.