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 "sensors.h"
34 
35 #include "param.h"
36 
37 #include "rosflight.h"
38 
39 #include <turbomath/turbomath.h>
40 
41 #include <cmath>
42 #include <cstdbool>
43 #include <cstdint>
44 
45 namespace rosflight_firmware
46 {
47 const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s
48 const float Sensors::BARO_SAMPLE_RATE = 50.0f;
49 const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2
50 const float Sensors::DIFF_SAMPLE_RATE = 50.0f;
51 const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s
52 const float Sensors::SONAR_SAMPLE_RATE = 50.0f;
53 
54 const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128;
55 const int Sensors::SENSOR_CAL_CYCLES = 127;
56 
57 const float Sensors::BARO_MAX_CALIBRATION_VARIANCE = 25.0; // standard dev about 0.2 m
58 const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard dev about 3 m/s
59 
60 Sensors::Sensors(ROSflight &rosflight) : rf_(rosflight) {}
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 
83 }
84 
86 {
87  // Quaternion to compensate for FCU orientation
88  float roll = rf_.params_.get_param_float(PARAM_FC_ROLL) * 0.017453293;
89  float pitch = rf_.params_.get_param_float(PARAM_FC_PITCH) * 0.017453293;
90  float yaw = rf_.params_.get_param_float(PARAM_FC_YAW) * 0.017453293;
91  data_.fcu_orientation = turbomath::Quaternion(roll, pitch, yaw);
92 
93  // See if the IMU is uncalibrated, and throw an error if it is
97  {
99  }
100 }
101 
102 void Sensors::param_change_callback(uint16_t param_id)
103 {
104  switch (param_id)
105  {
106  case PARAM_FC_ROLL:
107  case PARAM_FC_PITCH:
108  case PARAM_FC_YAW:
109  init_imu();
110  break;
114  break;
117  break;
120  break;
121  default:
122  // do nothing
123  break;
124  }
125 }
126 
127 bool Sensors::run(void)
128 {
129  // First, check for new IMU data
130  bool got_imu = update_imu();
131 
132  // Look for sensors that may not have been recognized at start because they weren't attached
133  // to the 5V rail (only if disarmed)
134  if (!rf_.state_manager_.state().armed)
136 
137  // Update other sensors
139  return got_imu;
140 }
141 
143 {
144  switch (next_sensor_to_update_)
145  {
146  case GNSS:
148  {
149  data_.gnss_present = true;
150  data_.gnss_new_data = true;
152  this->data_.gnss_data = rf_.board_.gnss_read();
154  }
155  break;
156 
157  case BAROMETER:
158  if (rf_.board_.baro_present())
159  {
160  data_.baro_present = true;
162  float raw_pressure;
163  float raw_temp;
164  rf_.board_.baro_read(&raw_pressure, &raw_temp);
166  if (data_.baro_valid)
167  {
168  data_.baro_temperature = raw_temp;
169  correct_baro();
170  }
171  }
172  break;
173 
174  case MAGNETOMETER:
175  if (rf_.board_.mag_present())
176  {
177  data_.mag_present = true;
178  float mag[3];
180  rf_.board_.mag_read(mag);
181  data_.mag.x = mag[0];
182  data_.mag.y = mag[1];
183  data_.mag.z = mag[2];
184  correct_mag();
185  }
186  break;
187 
188  case DIFF_PRESSURE:
190  {
191  // if diff_pressure is currently present OR if it has historically been
192  // present (diff_pressure_present default is false)
193  rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily
194  // disappears
195 
197  {
199  float raw_pressure;
200  float raw_temp;
201  rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp);
204  {
205  data_.diff_pressure_temp = raw_temp;
207  }
208  }
209  }
210  break;
211 
212  case SONAR:
214  if (rf_.board_.sonar_present())
215  {
216  data_.sonar_present = true;
217  float raw_distance;
219  raw_distance = rf_.board_.sonar_read();
221  }
222  break;
223  case BATTERY_MONITOR:
225  {
228  }
229  break;
230  default:
231  break;
232  }
233 
235 }
236 
238 {
239  // Look for disabled sensors while disarmed (poll every second)
240  // These sensors need power to respond, so they might not have been
241  // detected on startup, but will be detected whenever power is applied
242  // to the 5V rail.
244  {
250  }
251 }
252 
254 {
256 
257  calibrating_acc_flag_ = true;
261  return true;
262 }
263 
265 {
266  calibrating_gyro_flag_ = true;
270  return true;
271 }
272 
274 {
275  baro_calibration_mean_ = 0.0f;
276  baro_calibration_var_ = 0.0f;
278  baro_calibrated_ = false;
280  return true;
281 }
282 
284 {
290  return true;
291 }
292 
294 {
295  return !calibrating_gyro_flag_;
296 }
297 
298 //==================================================================
299 // local function definitions
301 {
302  if (rf_.board_.new_imu_data())
303  {
307  return false;
308 
309  // Move data into local copy
310  data_.accel.x = accel_[0];
311  data_.accel.y = accel_[1];
312  data_.accel.z = accel_[2];
313 
315 
316  data_.gyro.x = gyro_[0];
317  data_.gyro.y = gyro_[1];
318  data_.gyro.z = gyro_[2];
319 
321 
323  calibrate_accel();
325  calibrate_gyro();
326 
327  // Apply bias correction
328  correct_imu();
329 
330  // Integrate for filtered IMU
331  float dt = (data_.imu_time - prev_imu_read_time_us_) * 1e-6;
332  accel_int_ += dt * data_.accel;
333  gyro_int_ += dt * data_.gyro;
335 
336  return true;
337  }
338  else
339  {
340  // if we have lost 10 IMU messages then something is wrong
341  // However, because we look for disabled sensors while disarmed,
342  // we get IMU timeouts, which last for at least 10 ms. Therefore
343  // we have an adjustable imu_timeout.
344  int imu_timeout = rf_.state_manager_.state().armed ? 10 : 1000;
345  if (rf_.board_.clock_millis() > last_imu_update_ms_ + imu_timeout)
346  {
347  // Tell the board to fix it
349  if (!rf_.state_manager_.state().armed)
351 
352  // Indicate an IMU error
354  }
355  return false;
356  }
357 }
358 
360 {
361  float delta_t = (data_.imu_time - int_start_us_) * 1e-6;
362  accel = accel_int_ / delta_t;
363  gyro = gyro_int_ / delta_t;
364  accel_int_ *= 0.0;
365  gyro_int_ *= 0.0;
367  stamp_us = data_.imu_time;
368 }
369 
371 {
373  {
377  }
379  {
383  }
384 }
385 
386 //======================================================================
387 // Calibration Functions
389 {
390  gyro_sum_ += data_.gyro;
392 
393  if (gyro_calibration_count_ > 1000)
394  {
395  // Gyros are simple. Just find the average during the calibration
396  turbomath::Vector gyro_bias = gyro_sum_ / static_cast<float>(gyro_calibration_count_);
397 
398  if (gyro_bias.norm() < 1.0)
399  {
403 
404  // Tell the estimator to reset it's bias estimate, because it should be zero now
406 
407  // Tell the state manager that we just completed a gyro calibration
409  }
410  else
411  {
412  // Tell the state manager that we just failed a gyro calibration
414  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for gyro cal");
415  }
416 
417  // reset calibration in case we do it again
418  calibrating_gyro_flag_ = false;
420  gyro_sum_.x = 0.0f;
421  gyro_sum_.y = 0.0f;
422  gyro_sum_.z = 0.0f;
423  }
424 }
425 
427 {
428  return turbomath::Vector(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, a.z > b.z ? a.z : b.z);
429 }
430 
432 {
433  return turbomath::Vector(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, a.z < b.z ? a.z : b.z);
434 }
435 
437 {
443 
444  if (accel_calibration_count_ > 1000)
445  {
446  // The temperature bias is calculated using a least-squares regression.
447  // This is computationally intensive, so it is done by the companion
448  // computer in fcu_io and shipped over to the flight controller.
452 
453  // Figure out the proper accel bias.
454  // We have to consider the contribution of temperature during the calibration,
455  // Which is why this line is so confusing. What we are doing, is first removing
456  // the contribution of temperature to the measurements during the calibration,
457  // Then we are dividing by the number of measurements.
458  turbomath::Vector accel_bias =
459  (acc_sum_ - (accel_temp_bias * acc_temp_sum_)) / static_cast<float>(accel_calibration_count_);
460 
461  // Sanity Check -
462  // If the accelerometer is upside down or being spun around during the calibration,
463  // then don't do anything
464  if ((max_ - min_).norm() > 1.0)
465  {
466  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for IMU cal");
467  calibrating_acc_flag_ = false;
468  }
469  else
470  {
471  // reset the estimated state
473  calibrating_acc_flag_ = false;
474 
475  if (accel_bias.norm() < 3.0)
476  {
481 
482  // clear uncalibrated IMU flag
484  }
485  else
486  {
487  // This usually means the user has the FCU in the wrong orientation, or something is wrong
488  // with the board IMU (like it's a cheap chinese clone)
489  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "large accel bias: norm = %d.%d",
490  static_cast<uint32_t>(accel_bias.norm()),
491  static_cast<uint32_t>(accel_bias.norm() * 1000) % 1000);
492  }
493  }
494 
495  // reset calibration counters in case we do it again
497  acc_sum_.x = 0.0f;
498  acc_sum_.y = 0.0f;
499  acc_sum_.z = 0.0f;
500  acc_temp_sum_ = 0.0f;
501  max_.x = -1000.0f;
502  max_.y = -1000.0f;
503  max_.z = -1000.0f;
504  min_.x = 1000.0f;
505  min_.y = 1000.0f;
506  min_.z = 1000.0f;
507  }
508 }
509 
511 {
513  {
515 
516  // calibrate pressure reading to where it should be
518  {
519  // if sample variance within acceptable range, flag calibration as done
520  // else reset cal variables and start over
522  {
524  baro_calibrated_ = true;
526  }
527  else
528  {
529  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for barometer cal");
530  }
531  baro_calibration_mean_ = 0.0f;
532  baro_calibration_var_ = 0.0f;
534  }
535 
537  {
538  float measurement = data_.baro_pressure - ground_pressure_;
539  float delta = measurement - baro_calibration_mean_;
540  baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES);
541  float delta2 = measurement - baro_calibration_mean_;
542  baro_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1);
543  }
545  }
546 }
547 
549 {
551  {
553 
555  {
556  // if sample variance within acceptable range, flag calibration as done
557  // else reset cal variables and start over
559  {
562  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Airspeed Cal Successful!");
563  }
564  else
565  {
566  rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for diff pressure cal");
567  }
571  }
573  {
577  diff_pressure_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1);
578  }
580  }
581 }
582 
583 //======================================================
584 // Correction Functions (These apply calibration constants)
586 {
587  // correct according to known biases and temperature compensation
594 
598 }
599 
601 {
602  // correct according to known hard iron bias
603  float mag_hard_x = data_.mag.x - rf_.params_.get_param_float(PARAM_MAG_X_BIAS);
604  float mag_hard_y = data_.mag.y - rf_.params_.get_param_float(PARAM_MAG_Y_BIAS);
605  float mag_hard_z = data_.mag.z - rf_.params_.get_param_float(PARAM_MAG_Z_BIAS);
606 
607  // correct according to known soft iron bias - converts to nT
617 }
618 
620 {
621  if (!baro_calibrated_)
622  calibrate_baro();
625 }
626 
628 {
632  float atm = 101325.0f;
633  if (data_.baro_present)
634  atm = data_.baro_pressure;
638 }
639 
640 void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, float center)
641 {
642  max_change_ = max_change_rate / update_rate;
643  window_size_ = 1;
644  center_ = center;
645  init_ = true;
646 }
647 
648 bool Sensors::OutlierFilter::update(float new_val, float *val)
649 {
650  float diff = new_val - center_;
651  if (fabsf(diff) < window_size_ * max_change_)
652  {
653  *val = new_val;
654 
655  center_ += turbomath::fsign(diff) * fminf(max_change_, fabsf(diff));
656  if (window_size_ > 1)
657  {
658  window_size_--;
659  }
660  return true;
661  }
662  else
663  {
664  window_size_++;
665  return false;
666  }
667 }
668 
670 {
671  float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER);
672  float current_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER);
673  this->rf_.board_.battery_voltage_set_multiplier(voltage_multiplier);
674  this->rf_.board_.battery_current_set_multiplier(current_multiplier);
675 }
676 
677 } // namespace rosflight_firmware
const turbomath::Vector gravity_
Definition: sensors.h:264
virtual void battery_current_set_multiplier(double multiplier)=0
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:431
uint16_t baro_calibration_count_
Definition: sensors.h:278
virtual void sensors_init()=0
turbomath::Quaternion fcu_orientation
Definition: sensors.h:130
static const float DIFF_SAMPLE_RATE
Definition: sensors.h:195
static const float SONAR_SAMPLE_RATE
Definition: sensors.h:197
void param_change_callback(uint16_t param_id) override
Definition: sensors.cpp:102
virtual bool battery_voltage_present() const =0
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:426
static const float DIFF_MAX_CHANGE_RATE
Definition: sensors.h:194
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
void correct_diff_pressure(void)
Definition: sensors.cpp:627
turbomath::Vector gyro_sum_
Definition: sensors.h:261
static const float BARO_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:200
float fsign(float y)
Definition: turbomath.cpp:343
bool start_imu_calibration(void)
Definition: sensors.cpp:253
virtual void diff_pressure_update()=0
float alt(float press)
Definition: turbomath.cpp:470
virtual bool new_imu_data()=0
float fabs(float x)
Definition: turbomath.cpp:491
virtual void sonar_update()=0
uint32_t last_baro_cal_iter_ms_
Definition: sensors.h:279
OutlierFilter baro_outlier_filt_
Definition: sensors.h:291
static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE
Definition: sensors.h:201
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:378
uint16_t diff_pressure_calibration_count_
Definition: sensors.h:285
virtual void diff_pressure_read(float *diff_pressure, float *temperature)=0
virtual void mag_update()=0
float diff_pressure_calibration_mean_
Definition: sensors.h:287
bool update(float new_val, float *val)
Definition: sensors.cpp:648
OutlierFilter diff_outlier_filt_
Definition: sensors.h:292
virtual void baro_read(float *pressure, float *temperature)=0
virtual void gnss_update()=0
virtual bool sonar_present()=0
void update_battery_monitor(void)
Definition: sensors.cpp:370
void calibrate_diff_pressure(void)
Definition: sensors.cpp:548
virtual void battery_voltage_set_multiplier(double multiplier)=0
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
uint64_t prev_imu_read_time_us_
Definition: sensors.h:273
virtual void mag_read(float mag[3])=0
turbomath::Vector min_
Definition: sensors.h:267
virtual GNSSData gnss_read()=0
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
uint32_t last_battery_monitor_update_ms_
Definition: sensors.h:295
static const float BARO_SAMPLE_RATE
Definition: sensors.h:193
turbomath::Vector max_
Definition: sensors.h:266
static const int SENSOR_CAL_CYCLES
Definition: sensors.h:199
bool start_gyro_calibration(void)
Definition: sensors.cpp:264
bool start_baro_calibration(void)
Definition: sensors.cpp:273
bool gyro_calibration_complete(void)
Definition: sensors.cpp:293
virtual uint64_t clock_micros()=0
const State & state() const
void set_error(uint16_t error)
float diff_pressure_calibration_var_
Definition: sensors.h:288
virtual void baro_update()=0
uint32_t last_diff_pressure_cal_iter_ms_
Definition: sensors.h:286
void clear_error(uint16_t error)
static const float BARO_MAX_CHANGE_RATE
Definition: sensors.h:192
turbomath::Vector accel_int_
Definition: sensors.h:270
virtual bool gnss_present()=0
void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us)
Definition: sensors.cpp:359
uint16_t gyro_calibration_count_
Definition: sensors.h:260
void init(float max_change_rate, float update_rate, float center)
Definition: sensors.cpp:640
void look_for_disabled_sensors(void)
Definition: sensors.cpp:237
float inv_sqrt(float x)
Definition: turbomath.cpp:499
virtual bool battery_current_present() const =0
virtual uint32_t clock_millis()=0
uint16_t accel_calibration_count_
Definition: sensors.h:262
Sensors(ROSflight &rosflight)
Definition: sensors.cpp:60
float norm() const
Definition: turbomath.cpp:42
turbomath::Vector gyro_int_
Definition: sensors.h:271
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:283
virtual float battery_current_read() const =0
virtual GNSSFull gnss_full_read()=0
turbomath::Vector acc_sum_
Definition: sensors.h:263
uint32_t last_time_look_for_disarmed_sensors_
Definition: sensors.h:253
virtual bool baro_present()=0
virtual bool mag_present()=0
static const float SONAR_MAX_CHANGE_RATE
Definition: sensors.h:196
void update_battery_monitor_multipliers(void)
Definition: sensors.cpp:669
static const int SENSOR_CAL_DELAY_CYCLES
Definition: sensors.h:198
virtual float battery_voltage_read() const =0
void update_other_sensors(void)
Definition: sensors.cpp:142
OutlierFilter sonar_outlier_filt_
Definition: sensors.h:293
static constexpr uint32_t BATTERY_MONITOR_UPDATE_PERIOD_MS
Definition: sensors.h:202


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