estimator.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #ifndef ROSFLIGHT_FIRMWARE_ESTIMATOR_H
33 #define ROSFLIGHT_FIRMWARE_ESTIMATOR_H
34 
36 
37 #include <turbomath/turbomath.h>
38 
39 #include <cmath>
40 #include <cstdbool>
41 #include <cstdint>
42 
43 namespace rosflight_firmware
44 {
45 class ROSflight;
46 
48 {
49 public:
50  struct State
51  {
54  float roll;
55  float pitch;
56  float yaw;
57  uint64_t timestamp_us;
58  };
59 
60  Estimator(ROSflight& _rf);
61 
62  inline const State& state() const { return state_; }
63 
64  inline const turbomath::Vector& bias() { return bias_; }
65 
66  inline const turbomath::Vector& accLPF() { return accel_LPF_; }
67 
68  inline const turbomath::Vector& gyroLPF() { return gyro_LPF_; }
69 
70  void init();
71  void param_change_callback(uint16_t param_id) override;
72  void run();
73  void reset_state();
74  void reset_adaptive_bias();
76 
77 private:
78  const turbomath::Vector g_ = {0.0f, 0.0f, -1.0f};
79 
82 
83  uint64_t last_time_;
86 
89 
91 
94 
96 
99 
100  void run_LPF();
101 
102  bool can_use_accel() const;
103  bool can_use_extatt() const;
107  void integrate_angular_rate(turbomath::Quaternion& quat, const turbomath::Vector& omega, const float dt) const;
111  turbomath::Vector& Z) const;
112 };
113 
114 } // namespace rosflight_firmware
115 
116 #endif // ROSFLIGHT_FIRMWARE_ESTIMATOR_H
const turbomath::Vector & accLPF()
Definition: estimator.h:66
void quaternion_to_dcm(const turbomath::Quaternion &q, turbomath::Vector &X, turbomath::Vector &Y, turbomath::Vector &Z) const
Definition: estimator.cpp:378
void integrate_angular_rate(turbomath::Quaternion &quat, const turbomath::Vector &omega, const float dt) const
Definition: estimator.cpp:335
turbomath::Vector bias_
Definition: estimator.h:90
turbomath::Vector w_acc_
Definition: estimator.h:95
turbomath::Vector gyro_LPF_
Definition: estimator.h:93
turbomath::Vector accel_LPF_
Definition: estimator.h:92
turbomath::Vector smoothed_gyro_measurement()
Definition: estimator.cpp:316
void param_change_callback(uint16_t param_id) override
Definition: estimator.cpp:98
turbomath::Vector accel_correction() const
Definition: estimator.cpp:270
turbomath::Vector extatt_correction() const
Definition: estimator.cpp:293
const turbomath::Vector & bias()
Definition: estimator.h:64
const State & state() const
Definition: estimator.h:62
void set_external_attitude_update(const turbomath::Quaternion &q)
Definition: estimator.cpp:119
const turbomath::Vector & gyroLPF()
Definition: estimator.h:68
turbomath::Quaternion q_extatt_
Definition: estimator.h:98
turbomath::Vector w2_
Definition: estimator.h:88
turbomath::Quaternion attitude
Definition: estimator.h:53
turbomath::Vector angular_velocity
Definition: estimator.h:52
turbomath::Vector w1_
Definition: estimator.h:87
const turbomath::Vector g_
Definition: estimator.h:78


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