sensors.cpp
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 #include <stdbool.h>
34 #include <stdint.h>
35 #include <math.h>
36 
37 #include "sensors.h"
38 #include "rosflight.h"
39 
40 #include <turbomath/turbomath.h>
41 
42 namespace rosflight_firmware
43 {
44 
45 const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s
46 const float Sensors::BARO_SAMPLE_RATE = 50.0f;
47 const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2
48 const float Sensors::DIFF_SAMPLE_RATE = 50.0f;
49 const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s
50 const float Sensors::SONAR_SAMPLE_RATE = 50.0f;
51 
52 const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128;
53 const int Sensors::SENSOR_CAL_CYCLES = 127;
54 
55 const float Sensors::BARO_MAX_CALIBRATION_VARIANCE = 25.0; // standard dev about 0.2 m
56 const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard dev about 3 m/s
57 
59  rf_(rosflight)
60 {}
61 
63 {
64  new_imu_data_ = false;
65 
66  // clear the IMU read error
69 
70  init_imu();
71 
73 
75  ground_pressure_ = 101325.0f*static_cast<float>(pow((1-2.25694e-5 * alt), 5.2553));
76 
81 }
82 
84 {
85  // Quaternion to compensate for FCU orientation
86  float roll = rf_.params_.get_param_float(PARAM_FC_ROLL) * 0.017453293;
87  float pitch = rf_.params_.get_param_float(PARAM_FC_PITCH) * 0.017453293;
88  float yaw = rf_.params_.get_param_float(PARAM_FC_YAW) * 0.017453293;
89  data_.fcu_orientation = turbomath::Quaternion(roll, pitch, yaw);
90 
91  // See if the IMU is uncalibrated, and throw an error if it is
95  {
97  }
98 }
99 
100 void Sensors::param_change_callback(uint16_t param_id)
101 {
102  switch (param_id)
103  {
104  case PARAM_FC_ROLL:
105  case PARAM_FC_PITCH:
106  case PARAM_FC_YAW:
107  init_imu();
108  break;
109  default:
110  // do nothing
111  break;
112  }
113 }
114 
115 
116 bool Sensors::run(void)
117 {
118  // First, check for new IMU data
119  bool got_imu = update_imu();
120 
121  // Look for sensors that may not have been recognized at start because they weren't attached
122  // to the 5V rail (only if disarmed)
123  if (!rf_.state_manager_.state().armed)
125 
126  // Update other sensors
128  return got_imu;
129 }
130 
131 
133 {
134  switch (next_sensor_to_update_)
135  {
136  case GNSS:
138  {
139  data_.gnss_present = true;
140  data_.gnss_new_data = true;
142  this->data_.gnss_data = rf_.board_.gnss_read();
144  }
145  break;
146 
147  case BAROMETER:
148  if (rf_.board_.baro_present())
149  {
150  data_.baro_present = true;
152  float raw_pressure;
153  float raw_temp;
154  rf_.board_.baro_read(&raw_pressure, &raw_temp);
156  if (data_.baro_valid)
157  {
158  data_.baro_temperature = raw_temp;
159  correct_baro();
160  }
161  }
162  break;
163 
164  case MAGNETOMETER:
165  if (rf_.board_.mag_present())
166  {
167  data_.mag_present = true;
168  float mag[3];
170  rf_.board_.mag_read(mag);
171  data_.mag.x = mag[0];
172  data_.mag.y = mag[1];
173  data_.mag.z = mag[2];
174  correct_mag();
175  }
176  break;
177 
178  case DIFF_PRESSURE:
180  {
181  // if diff_pressure is currently present OR if it has historically been
182  // present (diff_pressure_present default is false)
183  rf_.board_.diff_pressure_update(); //update assists in recovering sensor if it temporarily disappears
184 
186  {
188  float raw_pressure;
189  float raw_temp;
190  rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp);
193  {
194  data_.diff_pressure_temp = raw_temp;
196  }
197  }
198  }
199  break;
200 
201  case SONAR:
203  if (rf_.board_.sonar_present())
204  {
205  data_.sonar_present = true;
206  float raw_distance;
208  raw_distance = rf_.board_.sonar_read();
210  }
211  break;
212  default:
213  break;
214  }
215 
217 }
218 
219 
221 {
222  // Look for disabled sensors while disarmed (poll every second)
223  // These sensors need power to respond, so they might not have been
224  // detected on startup, but will be detected whenever power is applied
225  // to the 5V rail.
227  {
233  }
234 }
235 
237 {
239 
240  calibrating_acc_flag_ = true;
244  return true;
245 }
246 
248 {
249  calibrating_gyro_flag_ = true;
253  return true;
254 }
255 
257 {
258  baro_calibration_mean_ = 0.0f;
259  baro_calibration_var_ = 0.0f;
261  baro_calibrated_ = false;
263  return true;
264 }
265 
267 {
273  return true;
274 }
275 
277 {
278  return !calibrating_gyro_flag_;
279 }
280 
281 //==================================================================
282 // local function definitions
284 {
285  if (rf_.board_.new_imu_data())
286  {
290  return false;
291 
292  // Move data into local copy
293  data_.accel.x = accel_[0];
294  data_.accel.y = accel_[1];
295  data_.accel.z = accel_[2];
296 
298 
299  data_.gyro.x = gyro_[0];
300  data_.gyro.y = gyro_[1];
301  data_.gyro.z = gyro_[2];
302 
304 
306  calibrate_accel();
308  calibrate_gyro();
309 
310  // Apply bias correction
311  correct_imu();
312 
313  // Integrate for filtered IMU
314  float dt = (data_.imu_time - prev_imu_read_time_us_) * 1e-6;
315  accel_int_ += dt * data_.accel;
316  gyro_int_ += dt * data_.gyro;
318 
319  return true;
320  }
321  else
322  {
323  // if we have lost 10 IMU messages then something is wrong
324  // However, because we look for disabled sensors while disarmed,
325  // we get IMU timeouts, which last for at least 10 ms. Therefore
326  // we have an adjustable imu_timeout.
327  int imu_timeout = rf_.state_manager_.state().armed ? 10 : 1000;
328  if (rf_.board_.clock_millis() > last_imu_update_ms_ + imu_timeout)
329  {
330  // Tell the board to fix it
332  if (!rf_.state_manager_.state().armed)
334 
335  // Indicate an IMU error
337  }
338  return false;
339  }
340 }
341 
342 
344 {
345  float delta_t = (data_.imu_time - int_start_us_)*1e-6;
346  accel = accel_int_ / delta_t;
347  gyro = gyro_int_ / delta_t;
348  accel_int_ *= 0.0;
349  gyro_int_ *= 0.0;
351  stamp_us = data_.imu_time;
352 }
353 
354 //======================================================================
355 // Calibration Functions
357 {
358  gyro_sum_ += data_.gyro;
360 
361  if (gyro_calibration_count_ > 1000)
362  {
363  // Gyros are simple. Just find the average during the calibration
364  turbomath::Vector gyro_bias = gyro_sum_ / static_cast<float>(gyro_calibration_count_);
365 
366  if (gyro_bias.norm() < 1.0)
367  {
371 
372  // Tell the estimator to reset it's bias estimate, because it should be zero now
374 
375  // Tell the state manager that we just completed a gyro calibration
377  }
378  else
379  {
380  // Tell the state manager that we just failed a gyro calibration
382  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for gyro cal");
383  }
384 
385  // reset calibration in case we do it again
386  calibrating_gyro_flag_ = false;
388  gyro_sum_.x = 0.0f;
389  gyro_sum_.y = 0.0f;
390  gyro_sum_.z = 0.0f;
391  }
392 }
393 
395 {
396  return turbomath::Vector(a.x > b.x ? a.x : b.x,
397  a.y > b.y ? a.y : b.y,
398  a.z > b.z ? a.z : b.z);
399 }
400 
402 {
403  return turbomath::Vector(a.x < b.x ? a.x : b.x,
404  a.y < b.y ? a.y : b.y,
405  a.z < b.z ? a.z : b.z);
406 }
407 
408 
410 {
416 
417  if (accel_calibration_count_ > 1000)
418  {
419  // The temperature bias is calculated using a least-squares regression.
420  // This is computationally intensive, so it is done by the companion
421  // computer in fcu_io and shipped over to the flight controller.
422  turbomath::Vector accel_temp_bias =
423  {
427  };
428 
429  // Figure out the proper accel bias.
430  // We have to consider the contribution of temperature during the calibration,
431  // Which is why this line is so confusing. What we are doing, is first removing
432  // the contribution of temperature to the measurements during the calibration,
433  // Then we are dividing by the number of measurements.
434  turbomath::Vector accel_bias = (acc_sum_ - (accel_temp_bias * acc_temp_sum_)) /
435  static_cast<float>(accel_calibration_count_);
436 
437  // Sanity Check -
438  // If the accelerometer is upside down or being spun around during the calibration,
439  // then don't do anything
440  if ((max_- min_).norm() > 1.0)
441  {
442  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for IMU cal");
443  calibrating_acc_flag_ = false;
444  }
445  else
446  {
447  // reset the estimated state
449  calibrating_acc_flag_ = false;
450 
451  if (accel_bias.norm() < 3.0)
452  {
457 
458  // clear uncalibrated IMU flag
460  }
461  else
462  {
463  // This usually means the user has the FCU in the wrong orientation, or something is wrong
464  // with the board IMU (like it's a cheap chinese clone)
465  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "large accel bias: norm = %d.%d",
466  static_cast<uint32_t>(accel_bias.norm()),
467  static_cast<uint32_t>(accel_bias.norm()*1000)%1000);
468  }
469  }
470 
471  // reset calibration counters in case we do it again
473  acc_sum_.x = 0.0f;
474  acc_sum_.y = 0.0f;
475  acc_sum_.z = 0.0f;
476  acc_temp_sum_ = 0.0f;
477  max_.x = -1000.0f;
478  max_.y = -1000.0f;
479  max_.z = -1000.0f;
480  min_.x = 1000.0f;
481  min_.y = 1000.0f;
482  min_.z = 1000.0f;
483  }
484 }
485 
487 {
489  {
491 
492  // calibrate pressure reading to where it should be
494  {
495  // if sample variance within acceptable range, flag calibration as done
496  // else reset cal variables and start over
498  {
500  baro_calibrated_ = true;
502  }
503  else
504  {
505  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for barometer cal");
506  }
507  baro_calibration_mean_ = 0.0f;
508  baro_calibration_var_ = 0.0f;
510  }
511 
513  {
514  float measurement = data_.baro_pressure - ground_pressure_;
515  float delta = measurement - baro_calibration_mean_;
516  baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES);
517  float delta2 = measurement - baro_calibration_mean_;
518  baro_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1);
519  }
521  }
522 }
523 
525 {
527  {
529 
531  {
532  // if sample variance within acceptable range, flag calibration as done
533  // else reset cal variables and start over
535  {
538  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Airspeed Cal Successful!");
539  }
540  else
541  {
542  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for diff pressure cal");
543  }
547  }
549  {
553  diff_pressure_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1);
554  }
556  }
557 }
558 
559 
560 //======================================================
561 // Correction Functions (These apply calibration constants)
563 {
564  // correct according to known biases and temperature compensation
571 
575 }
576 
578 {
579  // correct according to known hard iron bias
580  float mag_hard_x = data_.mag.x - rf_.params_.get_param_float(PARAM_MAG_X_BIAS);
581  float mag_hard_y = data_.mag.y - rf_.params_.get_param_float(PARAM_MAG_Y_BIAS);
582  float mag_hard_z = data_.mag.z - rf_.params_.get_param_float(PARAM_MAG_Z_BIAS);
583 
584  // correct according to known soft iron bias - converts to nT
586  PARAM_MAG_A12_COMP)*mag_hard_y +
589  PARAM_MAG_A22_COMP)*mag_hard_y +
592  PARAM_MAG_A32_COMP)*mag_hard_y +
594 }
595 
597 {
598  if (!baro_calibrated_)
599  calibrate_baro();
602 }
603 
605 {
609  float atm = 101325.0f;
610  if (data_.baro_present)
611  atm = data_.baro_pressure;
614 }
615 
616 void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, float center)
617 {
618  max_change_ = max_change_rate / update_rate;
619  window_size_ = 1;
620  center_ = center;
621  init_ = true;
622 }
623 
624 bool Sensors::OutlierFilter::update(float new_val, float *val)
625 {
626  float diff = new_val - center_;
627  if (fabsf(diff) < window_size_ * max_change_)
628  {
629  *val = new_val;
630 
631  center_ += turbomath::fsign(diff) * fminf(max_change_, fabsf(diff));
632  if (window_size_ > 1)
633  {
634  window_size_--;
635  }
636  return true;
637  }
638  else
639  {
640  window_size_++;
641  return false;
642  }
643 }
644 
645 } // namespace rosflight_firmware
const turbomath::Vector gravity_
Definition: sensors.h:262
virtual bool gnss_has_new_data()=0
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
turbomath::Vector vector_min(turbomath::Vector a, turbomath::Vector b)
Definition: sensors.cpp:401
uint16_t baro_calibration_count_
Definition: sensors.h:276
virtual void sensors_init()=0
turbomath::Quaternion fcu_orientation
Definition: sensors.h:136
static const float DIFF_SAMPLE_RATE
Definition: sensors.h:197
static const float SONAR_SAMPLE_RATE
Definition: sensors.h:199
void param_change_callback(uint16_t param_id) override
Definition: sensors.cpp:100
virtual void imu_not_responding_error()=0
virtual bool diff_pressure_present()=0
turbomath::Vector vector_max(turbomath::Vector a, turbomath::Vector b)
Definition: sensors.cpp:394
static const float DIFF_MAX_CHANGE_RATE
Definition: sensors.h:196
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
void correct_diff_pressure(void)
Definition: sensors.cpp:604
turbomath::Vector gyro_sum_
Definition: sensors.h:259
static const float BARO_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:202
float fsign(float y)
Definition: turbomath.cpp:392
bool start_imu_calibration(void)
Definition: sensors.cpp:236
virtual void diff_pressure_update()=0
float alt(float press)
Definition: turbomath.cpp:518
virtual bool new_imu_data()=0
float fabs(float x)
Definition: turbomath.cpp:538
virtual void sonar_update()=0
uint32_t last_baro_cal_iter_ms_
Definition: sensors.h:277
OutlierFilter baro_outlier_filt_
Definition: sensors.h:289
static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:203
bool set_param_float(uint16_t id, float value)
Sets the value of a floating point parameter by ID and calls the parameter callback.
Definition: param.cpp:369
uint16_t diff_pressure_calibration_count_
Definition: sensors.h:283
virtual void diff_pressure_read(float *diff_pressure, float *temperature)=0
virtual void mag_update()=0
float diff_pressure_calibration_mean_
Definition: sensors.h:285
bool update(float new_val, float *val)
Definition: sensors.cpp:624
OutlierFilter diff_outlier_filt_
Definition: sensors.h:290
virtual void baro_read(float *pressure, float *temperature)=0
virtual void gnss_update()=0
virtual bool sonar_present()=0
virtual GNSSRaw gnss_raw_read()=0
void calibrate_diff_pressure(void)
Definition: sensors.cpp:524
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:322
uint64_t prev_imu_read_time_us_
Definition: sensors.h:271
virtual void mag_read(float mag[3])=0
turbomath::Vector min_
Definition: sensors.h:265
virtual GNSSData gnss_read()=0
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
static const float BARO_SAMPLE_RATE
Definition: sensors.h:195
turbomath::Vector max_
Definition: sensors.h:264
static const int SENSOR_CAL_CYCLES
Definition: sensors.h:201
bool start_gyro_calibration(void)
Definition: sensors.cpp:247
bool start_baro_calibration(void)
Definition: sensors.cpp:256
bool gyro_calibration_complete(void)
Definition: sensors.cpp:276
virtual uint64_t clock_micros()=0
const State & state() const
Definition: state_manager.h:82
void set_error(uint16_t error)
rosflight_firmware::ROSflight * rosflight
float diff_pressure_calibration_var_
Definition: sensors.h:286
virtual void baro_update()=0
uint32_t last_diff_pressure_cal_iter_ms_
Definition: sensors.h:284
void clear_error(uint16_t error)
static const float BARO_MAX_CHANGE_RATE
Definition: sensors.h:194
turbomath::Vector accel_int_
Definition: sensors.h:268
virtual bool gnss_present()=0
void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us)
Definition: sensors.cpp:343
uint16_t gyro_calibration_count_
Definition: sensors.h:258
void init(float max_change_rate, float update_rate, float center)
Definition: sensors.cpp:616
void look_for_disabled_sensors(void)
Definition: sensors.cpp:220
float inv_sqrt(float x)
Definition: turbomath.cpp:547
virtual uint32_t clock_millis()=0
uint16_t accel_calibration_count_
Definition: sensors.h:260
Sensors(ROSflight &rosflight)
Definition: sensors.cpp:58
float norm() const
Definition: turbomath.cpp:46
turbomath::Vector gyro_int_
Definition: sensors.h:269
virtual float sonar_read()=0
virtual bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time)=0
bool start_diff_pressure_calibration(void)
Definition: sensors.cpp:266
turbomath::Vector acc_sum_
Definition: sensors.h:261
uint32_t last_time_look_for_disarmed_sensors_
Definition: sensors.h:251
virtual bool baro_present()=0
virtual bool mag_present()=0
static const float SONAR_MAX_CHANGE_RATE
Definition: sensors.h:198
static const int SENSOR_CAL_DELAY_CYCLES
Definition: sensors.h:200
void update_other_sensors(void)
Definition: sensors.cpp:132
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