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 
35 #include <stdint.h>
36 #include <stdbool.h>
37 #include <math.h>
38 
39 #include <turbomath/turbomath.h>
40 
42 
43 namespace rosflight_firmware
44 {
45 
46 class ROSflight;
47 
49 {
50 
51 public:
52  struct State
53  {
56  float roll;
57  float pitch;
58  float yaw;
59  uint64_t timestamp_us;
60  };
61 
62  Estimator(ROSflight &_rf);
63 
64  inline const State &state() const { return state_; }
65 
66  inline const turbomath::Vector& bias()
67  {
68  return bias_;
69  }
70 
71  inline const turbomath::Vector& accLPF()
72  {
73  return accel_LPF_;
74  }
75 
76  inline const turbomath::Vector& gyroLPF()
77  {
78  return gyro_LPF_;
79  }
80 
81  void init();
82  void param_change_callback(uint16_t param_id) override;
83  void run();
84  void reset_state();
85  void reset_adaptive_bias();
87 
88 private:
89  const turbomath::Vector g_ = {0.0f, 0.0f, -1.0f};
90 
93 
94  uint64_t last_time_;
96 
99 
101 
104 
106 
109 
110  void run_LPF();
111 };
112 
113 } // namespace rosflight_firmware
114 
115 #endif // ROSFLIGHT_FIRMWARE_ESTIMATOR_H
const turbomath::Vector & accLPF()
Definition: estimator.h:71
turbomath::Vector bias_
Definition: estimator.h:100
turbomath::Vector w_acc_
Definition: estimator.h:105
turbomath::Vector gyro_LPF_
Definition: estimator.h:103
turbomath::Vector accel_LPF_
Definition: estimator.h:102
void param_change_callback(uint16_t param_id) override
Definition: estimator.cpp:99
const turbomath::Vector & bias()
Definition: estimator.h:66
const State & state() const
Definition: estimator.h:64
void set_attitude_correction(const turbomath::Quaternion &q)
Definition: estimator.cpp:120
const turbomath::Vector & gyroLPF()
Definition: estimator.h:76
turbomath::Vector w2_
Definition: estimator.h:98
turbomath::Quaternion attitude
Definition: estimator.h:55
turbomath::Vector angular_velocity
Definition: estimator.h:54
turbomath::Vector w1_
Definition: estimator.h:97
turbomath::Quaternion q_correction_
Definition: estimator.h:108
const turbomath::Vector g_
Definition: estimator.h:89


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