sensors.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson, Daniel Koch, and Craig Bidstrup,
3  * BYU MAGICC Lab
4  *
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef ROSFLIGHT_FIRMWARE_SENSORS_H
34 #define ROSFLIGHT_FIRMWARE_SENSORS_H
35 
36 #include <cstdint>
37 #include <cstdbool>
38 #include <cstring>
39 #include <turbomath/turbomath.h>
40 
42 
43 namespace rosflight_firmware
44 {
45 // Fix type, as defined in sensor_msgs/NavSatStatus
47 {
48  GNSS_FIX_TYPE_NO_FIX, // Unable to fix position
49  GNSS_FIX_TYPE_FIX, // Unaugmented fix
50  GNSS_FIX_TYPE_SBAS_FIX, // with satellite-based augmentation
51  GNSS_FIX_TYPE_GBAS_FIX // with ground-based augmentation
52 };
53 
54 struct GNSSData
55 {
56  struct ECEF
57  {
58  int32_t x; // cm
59  int32_t y; // cm
60  int32_t z; // cm
61  uint32_t p_acc; // cm
62  int32_t vx; // cm/s
63  int32_t vy; // cm/s
64  int32_t vz; // cm/s
65  uint32_t s_acc; // cm/s
66  };
67 
69  uint32_t time_of_week;
70  uint64_t time; // Unix time, in seconds
71  uint64_t nanos; // Fractional time
72  int32_t lat; // deg*10^-7
73  int32_t lon; // deg*10^-7
74  int32_t height; // mm
75  int32_t vel_n; // mm/s
76  int32_t vel_e; // mm/s
77  int32_t vel_d; // mm/s
78  uint32_t h_acc; // mm
79  uint32_t v_acc; // mm
80 
82 
83  uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message
84 
86  {
87  memset(this, 0, sizeof(GNSSData));
88  }
89 };
90 
91 struct GNSSRaw
92 {
93  uint64_t time_of_week;
94  uint16_t year;
95  uint8_t month;
96  uint8_t day;
97  uint8_t hour;
98  uint8_t min;
99  uint8_t sec;
100  uint8_t valid;
101  uint32_t t_acc;
102  int32_t nano;
103  uint8_t fix_type;
104  uint8_t num_sat;
105  int32_t lon;
106  int32_t lat;
107  int32_t height;
108  int32_t height_msl;
109  uint32_t h_acc;
110  uint32_t v_acc;
111  int32_t vel_n;
112  int32_t vel_e;
113  int32_t vel_d;
114  int32_t g_speed;
115  int32_t head_mot;
116  uint32_t s_acc;
117  uint32_t head_acc;
118  uint16_t p_dop;
119  uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message
120 
122  {
123  memset(this, 0, sizeof(GNSSRaw));
124  }
125 };
126 
127 class ROSflight;
128 
130 {
131 public:
132  struct Data
133  {
135  turbomath::Vector gyro = {0, 0, 0};
136  turbomath::Quaternion fcu_orientation = {1, 0, 0, 0};
137  float imu_temperature = 0;
138  uint64_t imu_time = 0;
139 
140  float diff_pressure_velocity = 0;
141  float diff_pressure = 0;
142  float diff_pressure_temp = 0;
143  bool diff_pressure_valid = false;
144 
145  float baro_altitude = 0;
146  float baro_pressure = 0;
147  float baro_temperature = 0;
148  bool baro_valid = false;
149 
150  float sonar_range = 0;
151  bool sonar_range_valid = false;
152 
154  bool gnss_new_data = false;
155  float gps_CNO = 0; // What is this?
156  bool gnss_present = false;
158 
159  turbomath::Vector mag = {0, 0, 0};
160 
161  bool baro_present = false;
162  bool mag_present = false;
163  bool sonar_present = false;
164  bool diff_pressure_present = false;
165  };
166 
168 
169  inline const Data &data() const { return data_; }
170  void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us);
171 
172  // function declarations
173  void init();
174  bool run();
175  void param_change_callback(uint16_t param_id) override;
176 
177  // Calibration Functions
178  bool start_imu_calibration(void);
179  bool start_gyro_calibration(void);
180  bool start_baro_calibration(void);
181  bool start_diff_pressure_calibration(void);
182  bool gyro_calibration_complete(void);
183 
184  inline bool should_send_imu_data(void)
185  {
186  if (imu_data_sent_)
187  return false;
188  else
189  imu_data_sent_ = true;
190  return true;
191  }
192 
193 private:
194  static const float BARO_MAX_CHANGE_RATE;
195  static const float BARO_SAMPLE_RATE;
196  static const float DIFF_MAX_CHANGE_RATE;
197  static const float DIFF_SAMPLE_RATE;
198  static const float SONAR_MAX_CHANGE_RATE;
199  static const float SONAR_SAMPLE_RATE;
200  static const int SENSOR_CAL_DELAY_CYCLES;
201  static const int SENSOR_CAL_CYCLES;
202  static const float BARO_MAX_CALIBRATION_VARIANCE;
204 
206  {
207  private:
208  float max_change_;
209  float center_;
211  bool init_ = false;
212 
213  public:
215  void init(float max_change_rate, float update_rate, float center);
216  bool update(float new_val, float *val);
217  };
218 
219  enum : uint8_t
220  {
226  NUM_LOW_PRIORITY_SENSORS
227  };
228 
230 
232 
233  float accel_[3] = {0, 0, 0};
234  float gyro_[3] = {0, 0, 0};
235 
236  bool calibrating_acc_flag_ = false;
237  bool calibrating_gyro_flag_ = false;
238  uint8_t next_sensor_to_update_ = BAROMETER;
239  void init_imu();
240  void calibrate_accel(void);
241  void calibrate_gyro(void);
242  void calibrate_baro(void);
243  void calibrate_diff_pressure(void);
244  void correct_imu(void);
245  void correct_mag(void);
246  void correct_baro(void);
247  void correct_diff_pressure(void);
248  bool update_imu(void);
249  void update_other_sensors(void);
250  void look_for_disabled_sensors(void);
251  uint32_t last_time_look_for_disarmed_sensors_ = 0;
252  uint32_t last_imu_update_ms_ = 0;
253 
256 
257  // IMU calibration
258  uint16_t gyro_calibration_count_ = 0;
259  turbomath::Vector gyro_sum_ = {0, 0, 0};
260  uint16_t accel_calibration_count_ = 0;
261  turbomath::Vector acc_sum_ = {0, 0, 0};
262  const turbomath::Vector gravity_ = {0.0f, 0.0f, 9.80665f};
263  float acc_temp_sum_ = 0.0f;
264  turbomath::Vector max_ = {-1000.0f, -1000.0f, -1000.0f};
265  turbomath::Vector min_ = {1000.0f, 1000.0f, 1000.0f};
266 
267  // Filtered IMU
270  uint64_t int_start_us_;
272 
273  // Baro Calibration
274  bool baro_calibrated_ = false;
275  float ground_pressure_ = 0.0f;
276  uint16_t baro_calibration_count_ = 0;
277  uint32_t last_baro_cal_iter_ms_ = 0;
278  float baro_calibration_mean_ = 0.0f;
279  float baro_calibration_var_ = 0.0f;
280 
281  // Diff Pressure Calibration
282  bool diff_pressure_calibrated_ = false;
283  uint16_t diff_pressure_calibration_count_ = 0;
284  uint32_t last_diff_pressure_cal_iter_ms_ = 0;
285  float diff_pressure_calibration_mean_ = 0.0f;
286  float diff_pressure_calibration_var_ = 0.0f;
287 
288  // Sensor Measurement Outlier Filters
292 
293 };
294 
295 } // namespace rosflight_firmware
296 
297 #endif // ROSFLIGHT_FIRMWARE_SENSORS_H
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
static const float DIFF_SAMPLE_RATE
Definition: sensors.h:197
static const float SONAR_SAMPLE_RATE
Definition: sensors.h:199
static const float DIFF_MAX_CHANGE_RATE
Definition: sensors.h:196
void init(const M_string &remappings)
bool sonar_present
Definition: multisense.c:44
static const float BARO_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:202
OutlierFilter baro_outlier_filt_
Definition: sensors.h:289
static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:203
OutlierFilter diff_outlier_filt_
Definition: sensors.h:290
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
uint64_t prev_imu_read_time_us_
Definition: sensors.h:271
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
static const float BARO_SAMPLE_RATE
Definition: sensors.h:195
static const int SENSOR_CAL_CYCLES
Definition: sensors.h:201
rosflight_firmware::ROSflight * rosflight
bool should_send_imu_data(void)
Definition: sensors.h:184
static const float BARO_MAX_CHANGE_RATE
Definition: sensors.h:194
turbomath::Vector accel_int_
Definition: sensors.h:268
static volatile float data_[3]
Definition: drv_hmc5883l.c:75
turbomath::Vector gyro_int_
Definition: sensors.h:269
static const float SONAR_MAX_CHANGE_RATE
Definition: sensors.h:198
bool baro_present
Definition: multisense.c:42
const Data & data() const
Definition: sensors.h:169
static const int SENSOR_CAL_DELAY_CYCLES
Definition: sensors.h:200
bool mag_present
Definition: multisense.c:43
OutlierFilter sonar_outlier_filt_
Definition: sensors.h:291


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:19