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 
37 
38 #include <turbomath/turbomath.h>
39 
40 #include <cstdbool>
41 #include <cstdint>
42 #include <cstring>
43 
44 namespace rosflight_firmware
45 {
47 {
50  GNSS_FIX_RTK_FLOAT, // The two RTK fix types are for possible future use.
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 
85  GNSSData() { memset(this, 0, sizeof(GNSSData)); }
86 };
87 
88 struct GNSSFull
89 {
90  uint64_t time_of_week;
91  uint16_t year;
92  uint8_t month;
93  uint8_t day;
94  uint8_t hour;
95  uint8_t min;
96  uint8_t sec;
97  uint8_t valid;
98  uint32_t t_acc;
99  int32_t nano;
100  uint8_t fix_type;
101  uint8_t num_sat;
102  int32_t lon;
103  int32_t lat;
104  int32_t height;
105  int32_t height_msl;
106  uint32_t h_acc;
107  uint32_t v_acc;
108  int32_t vel_n;
109  int32_t vel_e;
110  int32_t vel_d;
111  int32_t g_speed;
112  int32_t head_mot;
113  uint32_t s_acc;
114  uint32_t head_acc;
115  uint16_t p_dop;
116  uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message
117 
118  GNSSFull() { memset(this, 0, sizeof(GNSSFull)); }
119 };
120 
121 class ROSflight;
122 
124 {
125 public:
126  struct Data
127  {
129  turbomath::Vector gyro = {0, 0, 0};
130  turbomath::Quaternion fcu_orientation = {1, 0, 0, 0};
131  float imu_temperature = 0;
132  uint64_t imu_time = 0;
133 
134  float diff_pressure_velocity = 0;
135  float diff_pressure = 0;
136  float diff_pressure_temp = 0;
137  bool diff_pressure_valid = false;
138 
139  float baro_altitude = 0;
140  float baro_pressure = 0;
141  float baro_temperature = 0;
142  bool baro_valid = false;
143 
144  float sonar_range = 0;
145  bool sonar_range_valid = false;
146 
148  bool gnss_new_data = false;
149  float gps_CNO = 0; // What is this?
150  bool gnss_present = false;
152 
153  turbomath::Vector mag = {0, 0, 0};
154 
155  bool baro_present = false;
156  bool mag_present = false;
157  bool sonar_present = false;
158  bool diff_pressure_present = false;
159 
160  bool battery_monitor_present = false;
161  float battery_voltage = 0;
162  float battery_current = 0;
163  };
164 
165  Sensors(ROSflight &rosflight);
166 
167  inline const Data &data() const { return data_; }
168  void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us);
169 
170  // function declarations
171  void init();
172  bool run();
173  void param_change_callback(uint16_t param_id) override;
174 
175  // Calibration Functions
176  bool start_imu_calibration(void);
177  bool start_gyro_calibration(void);
178  bool start_baro_calibration(void);
179  bool start_diff_pressure_calibration(void);
180  bool gyro_calibration_complete(void);
181 
182  inline bool should_send_imu_data(void)
183  {
184  if (imu_data_sent_)
185  return false;
186  else
187  imu_data_sent_ = true;
188  return true;
189  }
190 
191 private:
192  static const float BARO_MAX_CHANGE_RATE;
193  static const float BARO_SAMPLE_RATE;
194  static const float DIFF_MAX_CHANGE_RATE;
195  static const float DIFF_SAMPLE_RATE;
196  static const float SONAR_MAX_CHANGE_RATE;
197  static const float SONAR_SAMPLE_RATE;
198  static const int SENSOR_CAL_DELAY_CYCLES;
199  static const int SENSOR_CAL_CYCLES;
200  static const float BARO_MAX_CALIBRATION_VARIANCE;
202  static constexpr uint32_t BATTERY_MONITOR_UPDATE_PERIOD_MS = 10;
203 
205  {
206  private:
207  float max_change_;
208  float center_;
210  bool init_ = false;
211 
212  public:
214  void init(float max_change_rate, float update_rate, float center);
215  bool update(float new_val, float *val);
216  };
217 
218  enum : uint8_t
219  {
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_battery_monitor(void);
250  void update_other_sensors(void);
251  void look_for_disabled_sensors(void);
252  void update_battery_monitor_multipliers(void);
253  uint32_t last_time_look_for_disarmed_sensors_ = 0;
254  uint32_t last_imu_update_ms_ = 0;
255 
258 
259  // IMU calibration
260  uint16_t gyro_calibration_count_ = 0;
261  turbomath::Vector gyro_sum_ = {0, 0, 0};
262  uint16_t accel_calibration_count_ = 0;
263  turbomath::Vector acc_sum_ = {0, 0, 0};
264  const turbomath::Vector gravity_ = {0.0f, 0.0f, 9.80665f};
265  float acc_temp_sum_ = 0.0f;
266  turbomath::Vector max_ = {-1000.0f, -1000.0f, -1000.0f};
267  turbomath::Vector min_ = {1000.0f, 1000.0f, 1000.0f};
268 
269  // Filtered IMU
272  uint64_t int_start_us_;
274 
275  // Baro Calibration
276  bool baro_calibrated_ = false;
277  float ground_pressure_ = 0.0f;
278  uint16_t baro_calibration_count_ = 0;
279  uint32_t last_baro_cal_iter_ms_ = 0;
280  float baro_calibration_mean_ = 0.0f;
281  float baro_calibration_var_ = 0.0f;
282 
283  // Diff Pressure Calibration
284  bool diff_pressure_calibrated_ = false;
285  uint16_t diff_pressure_calibration_count_ = 0;
286  uint32_t last_diff_pressure_cal_iter_ms_ = 0;
287  float diff_pressure_calibration_mean_ = 0.0f;
288  float diff_pressure_calibration_var_ = 0.0f;
289 
290  // Sensor Measurement Outlier Filters
294 
295  uint32_t last_battery_monitor_update_ms_ = 0;
296  // Battery Monitor
297  float battery_voltage_alpha_{0.995};
298  float battery_current_alpha_{0.995};
299 };
300 
301 } // namespace rosflight_firmware
302 
303 #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:195
static const float SONAR_SAMPLE_RATE
Definition: sensors.h:197
static const float DIFF_MAX_CHANGE_RATE
Definition: sensors.h:194
void init(const M_string &remappings)
bool sonar_present
Definition: multisense.c:44
static const float BARO_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:200
OutlierFilter baro_outlier_filt_
Definition: sensors.h:291
static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:201
OutlierFilter diff_outlier_filt_
Definition: sensors.h:292
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
uint64_t prev_imu_read_time_us_
Definition: sensors.h:273
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
static const float BARO_SAMPLE_RATE
Definition: sensors.h:193
static const int SENSOR_CAL_CYCLES
Definition: sensors.h:199
bool should_send_imu_data(void)
Definition: sensors.h:182
static const float BARO_MAX_CHANGE_RATE
Definition: sensors.h:192
turbomath::Vector accel_int_
Definition: sensors.h:270
static volatile float data_[3]
Definition: drv_hmc5883l.c:75
turbomath::Vector gyro_int_
Definition: sensors.h:271
static const float SONAR_MAX_CHANGE_RATE
Definition: sensors.h:196
bool baro_present
Definition: multisense.c:42
const Data & data() const
Definition: sensors.h:167
static const int SENSOR_CAL_DELAY_CYCLES
Definition: sensors.h:198
bool mag_present
Definition: multisense.c:43
OutlierFilter sonar_outlier_filt_
Definition: sensors.h:293


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:16