Line data Source code
1 : /*
2 : APM_AHRS.cpp
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : #include "AP_AHRS.h"
18 : #include "AP_AHRS_View.h"
19 :
20 : #include <AP_Common/Location.h>
21 : #include <AP_HAL/AP_HAL.h>
22 : #include <AP_Logger/AP_Logger.h>
23 : #include <AP_GPS/AP_GPS.h>
24 : #include <AP_Baro/AP_Baro.h>
25 : #include <AP_Compass/AP_Compass.h>
26 : #include <AP_Vehicle/AP_Vehicle_Type.h>
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
31 38148955 : Vector3f AP_AHRS::get_gyro_latest(void) const
32 : {
33 38148955 : const uint8_t primary_gyro = get_primary_gyro_index();
34 38148955 : return AP::ins().get_gyro(primary_gyro) + get_gyro_drift();
35 : }
36 :
37 : // set_trim
38 31 : void AP_AHRS::set_trim(const Vector3f &new_trim)
39 : {
40 : const Vector3f trim {
41 31 : constrain_float(new_trim.x, radians(-AP_AHRS_TRIM_LIMIT), radians(AP_AHRS_TRIM_LIMIT)),
42 31 : constrain_float(new_trim.y, radians(-AP_AHRS_TRIM_LIMIT), radians(AP_AHRS_TRIM_LIMIT)),
43 31 : constrain_float(new_trim.z, radians(-AP_AHRS_TRIM_LIMIT), radians(AP_AHRS_TRIM_LIMIT))
44 31 : };
45 31 : _trim.set_and_save(trim);
46 31 : }
47 :
48 : // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
49 371 : void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
50 : {
51 371 : Vector3f trim = _trim.get();
52 :
53 : // add new trim
54 371 : trim.x = constrain_float(trim.x + roll_in_radians, radians(-AP_AHRS_TRIM_LIMIT), radians(AP_AHRS_TRIM_LIMIT));
55 371 : trim.y = constrain_float(trim.y + pitch_in_radians, radians(-AP_AHRS_TRIM_LIMIT), radians(AP_AHRS_TRIM_LIMIT));
56 :
57 : // set new trim values
58 371 : _trim.set(trim);
59 :
60 : // save to eeprom
61 371 : if( save_to_eeprom ) {
62 2 : _trim.save();
63 : }
64 371 : }
65 :
66 : // Set the board mounting orientation from AHRS_ORIENTATION parameter
67 41690897 : void AP_AHRS::update_orientation()
68 : {
69 41690897 : const uint32_t now_ms = AP_HAL::millis();
70 41690897 : if (last_orientation_update_ms != 0 &&
71 41688803 : now_ms - last_orientation_update_ms < 1000) {
72 : // only update once/second
73 14007242 : return;
74 : }
75 :
76 : // never update while armed - unless we've never updated
77 : // (e.g. mid-air reboot or ARMING_REQUIRED=NO on Plane):
78 27683655 : if (hal.util->get_soft_armed() && last_orientation_update_ms != 0) {
79 27629330 : return;
80 : }
81 :
82 54325 : last_orientation_update_ms = now_ms;
83 :
84 54325 : const enum Rotation orientation = (enum Rotation)_board_orientation.get();
85 :
86 54325 : AP::ins().set_board_orientation(orientation);
87 54325 : AP::compass().set_board_orientation(orientation);
88 : }
89 :
90 : /*
91 : calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
92 : */
93 80785613 : void AP_AHRS::calc_trig(const Matrix3f &rot,
94 : float &cr, float &cp, float &cy,
95 : float &sr, float &sp, float &sy) const
96 : {
97 80785613 : Vector2f yaw_vector(rot.a.x, rot.b.x);
98 :
99 80785613 : if (fabsf(yaw_vector.x) > 0 ||
100 19214 : fabsf(yaw_vector.y) > 0) {
101 80766439 : yaw_vector.normalize();
102 : }
103 80785613 : sy = constrain_float(yaw_vector.y, -1.0f, 1.0f);
104 80785613 : cy = constrain_float(yaw_vector.x, -1.0f, 1.0f);
105 :
106 : // sanity checks
107 80785613 : if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
108 0 : sy = 0.0f;
109 0 : cy = 1.0f;
110 : }
111 :
112 80785613 : const float cx2 = rot.c.x * rot.c.x;
113 80785613 : if (cx2 >= 1.0f) {
114 10463 : cp = 0;
115 10463 : cr = 1.0f;
116 : } else {
117 80775150 : cp = safe_sqrt(1 - cx2);
118 80775150 : cr = rot.c.z / cp;
119 : }
120 80785613 : cp = constrain_float(cp, 0.0f, 1.0f);
121 80785613 : cr = constrain_float(cr, -1.0f, 1.0f); // this relies on constrain_float() of infinity doing the right thing
122 :
123 80785613 : sp = -rot.c.x;
124 :
125 80785613 : if (!is_zero(cp)) {
126 80775150 : sr = rot.c.y / cp;
127 : }
128 :
129 80785613 : if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) {
130 : float r, p, y;
131 10463 : rot.to_euler(&r, &p, &y);
132 10463 : cr = cosf(r);
133 10463 : sr = sinf(r);
134 : }
135 80785613 : }
136 :
137 : // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
138 : // should be called after _dcm_matrix is updated
139 41686716 : void AP_AHRS::update_trig(void)
140 : {
141 41686716 : calc_trig(get_rotation_body_to_ned(),
142 41686716 : _cos_roll, _cos_pitch, _cos_yaw,
143 41686716 : _sin_roll, _sin_pitch, _sin_yaw);
144 41686716 : }
145 :
146 : /*
147 : update the centi-degree values
148 : */
149 41686716 : void AP_AHRS::update_cd_values(void)
150 : {
151 41686716 : roll_sensor = degrees(roll) * 100;
152 41686716 : pitch_sensor = degrees(pitch) * 100;
153 41686716 : yaw_sensor = degrees(yaw) * 100;
154 41686716 : if (yaw_sensor < 0)
155 25800575 : yaw_sensor += 36000;
156 :
157 41686716 : rpy_deg[0] = degrees(roll);
158 41686716 : rpy_deg[1] = degrees(pitch);
159 41686716 : rpy_deg[2] = wrap_360(degrees(yaw)); // we are probably already in trouble if this is required
160 41686716 : }
161 :
162 : /*
163 : create a rotated view of AP_AHRS with optional pitch trim
164 : */
165 1202 : AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg)
166 : {
167 1202 : if (_view != nullptr) {
168 : // can only have one
169 0 : return nullptr;
170 : }
171 1202 : _view = NEW_NOTHROW AP_AHRS_View(*this, rotation, pitch_trim_deg);
172 1202 : return _view;
173 : }
174 :
175 : /*
176 : * Update AOA and SSA estimation based on airspeed, velocity vector and wind vector
177 : *
178 : * Based on:
179 : * "On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors" by
180 : * Tor A. Johansen, Andrea Cristofaro, Kim Sorensen, Jakob M. Hansen, Thor I. Fossen
181 : *
182 : * "Multi-Stage Fusion Algorithm for Estimation of Aerodynamic Angles in Mini Aerial Vehicle" by
183 : * C.Ramprasadh and Hemendra Arya
184 : *
185 : * "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by
186 : * JOSEPH E. ZEIS, JR., CAPTAIN, USAF
187 : */
188 41686716 : void AP_AHRS::update_AOA_SSA(void)
189 : {
190 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
191 8377651 : const uint32_t now = AP_HAL::millis();
192 8377651 : if (now - _last_AOA_update_ms < 50) {
193 : // don't update at more than 20Hz
194 7750530 : return;
195 : }
196 989081 : _last_AOA_update_ms = now;
197 :
198 989081 : Vector3f aoa_velocity, aoa_wind;
199 :
200 : // get velocity and wind
201 989081 : if (get_velocity_NED(aoa_velocity) == false) {
202 57428 : return;
203 : }
204 :
205 : // use the wind estimate even if it is not marked valid; a zero or
206 : // stale wind vector simply degrades the AOA/SSA estimate
207 931653 : IGNORE_RETURN(get_wind(aoa_wind));
208 :
209 : // Rotate vectors to the body frame and calculate velocity and wind
210 931653 : const Matrix3f &rot = get_rotation_body_to_ned();
211 931653 : aoa_velocity = rot.mul_transpose(aoa_velocity);
212 931653 : aoa_wind = rot.mul_transpose(aoa_wind);
213 :
214 : // calculate relative velocity in body coordinates
215 931653 : aoa_velocity = aoa_velocity - aoa_wind;
216 931653 : const float vel_len = aoa_velocity.length();
217 :
218 : // do not calculate if speed is too low
219 931653 : if (vel_len < 2.0) {
220 304532 : _AOA = 0;
221 304532 : _SSA = 0;
222 304532 : return;
223 : }
224 :
225 : // Calculate AOA and SSA
226 627121 : if (aoa_velocity.x > 0) {
227 605088 : _AOA = degrees(atanf(aoa_velocity.z / aoa_velocity.x));
228 : } else {
229 22033 : _AOA = 0;
230 : }
231 :
232 627121 : _SSA = degrees(safe_asin(aoa_velocity.y / vel_len));
233 : #endif
234 33309065 : }
235 :
236 : // rotate a 2D vector from earth frame to body frame
237 2413790 : Vector2f AP_AHRS::earth_to_body2D(const Vector2f &ef) const
238 : {
239 4827580 : return Vector2f(ef.x * _cos_yaw + ef.y * _sin_yaw,
240 2413790 : -ef.x * _sin_yaw + ef.y * _cos_yaw);
241 : }
242 :
243 : // rotate a 2D vector from body frame to earth frame
244 400648 : Vector2f AP_AHRS::body_to_earth2D(const Vector2f &bf) const
245 : {
246 801296 : return Vector2f(bf.x * _cos_yaw - bf.y * _sin_yaw,
247 400648 : bf.x * _sin_yaw + bf.y * _cos_yaw);
248 : }
249 :
250 : // rotate a 2D vector from body frame to earth frame
251 1 : Vector2p AP_AHRS::body_to_earth2D_p(const Vector2p &bf) const
252 : {
253 2 : return Vector2p(bf.x * _cos_yaw - bf.y * _sin_yaw,
254 1 : bf.x * _sin_yaw + bf.y * _cos_yaw);
255 : }
256 :
257 : #if HAL_LOGGING_ENABLED
258 : // log ahrs home and EKF origin
259 12878 : void AP_AHRS::Log_Write_Home_And_Origin()
260 : {
261 12878 : AP_Logger *logger = AP_Logger::get_singleton();
262 12878 : if (logger == nullptr) {
263 4 : return;
264 : }
265 12874 : Location ekf_orig;
266 12874 : if (get_origin(ekf_orig)) {
267 8806 : Write_Origin(LogOriginType::ekf_origin, ekf_orig);
268 : }
269 :
270 12874 : if (_home_is_set) {
271 8845 : Write_Origin(LogOriginType::ahrs_home, _home);
272 : }
273 : }
274 : #endif
275 :
276 : // get apparent to true airspeed ratio
277 87174670 : float AP_AHRS_Backend::get_EAS2TAS(void) {
278 87174670 : return AP::baro().get_EAS2TAS();
279 : }
280 :
281 : // return current vibration vector for primary IMU
282 0 : Vector3f AP_AHRS::get_vibration(void) const
283 : {
284 0 : return AP::ins().get_vibration_levels();
285 : }
286 :
287 3830804 : void AP_AHRS::set_takeoff_expected(bool b)
288 : {
289 3830804 : takeoff_expected = b;
290 3830804 : takeoff_expected_start_ms = AP_HAL::millis();
291 3830804 : }
292 :
293 4162467 : void AP_AHRS::set_touchdown_expected(bool b)
294 : {
295 4162467 : touchdown_expected = b;
296 4162467 : touchdown_expected_start_ms = AP_HAL::millis();
297 4162467 : }
298 :
299 : /*
300 : update takeoff/touchdown flags
301 : */
302 41686716 : void AP_AHRS::update_flags(void)
303 : {
304 41686716 : const uint32_t timeout_ms = 1000;
305 41686716 : if (takeoff_expected && AP_HAL::millis() - takeoff_expected_start_ms > timeout_ms) {
306 173 : takeoff_expected = false;
307 : }
308 41686716 : if (touchdown_expected && AP_HAL::millis() - touchdown_expected_start_ms > timeout_ms) {
309 54 : touchdown_expected = false;
310 : }
311 41686716 : }
|
