Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
state.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2011-2012 Felix Ruess <felix.ruess@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
33
34#include "state.h"
35
36struct State state;
37
38#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
39/* Preflight checks */
41static struct preflight_check_t state_pfc;
42
43static void state_preflight(struct preflight_result_t *result) {
45 preflight_error(result, "State attitude is invalid");
46 } else {
47 preflight_success(result, "State attitude is valid");
48 }
49}
50#endif // PREFLIGHT_CHECKS
51
56
57void stateInit(void)
58{
59 state.pos_status = 0;
60 state.speed_status = 0;
61 state.accel_status = 0;
62 state.ned_to_body_orientation.status = 0;
63 state.rate_status = 0;
64 state.wind_air_status = 0;
65 state.ned_initialized_i = false;
66 state.ned_initialized_f = false;
67 state.utm_initialized_f = false;
68 state.origin_input_filter = STATE_INPUT_ANY;
69 state.pos_input_filter = STATE_INPUT_ANY;
70 state.speed_input_filter = STATE_INPUT_ANY;
71 state.accel_input_filter = STATE_INPUT_ANY;
72 state.attitude_input_filter = STATE_INPUT_ANY;
73 state.rates_input_filter = STATE_INPUT_ANY;
74 state.wind_air_input_filter = STATE_INPUT_ANY;
75
76#ifdef FIXEDWING_FIRMWARE
77 state.fixed_wing_flight = true;
78#else
79 state.fixed_wing_flight = false;
80#endif
81
82 /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
83 state.utm_origin_f.zone = 0;
84
85 /* Register preflight checks */
86#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
87 preflight_check_register(&state_pfc, state_preflight);
88#endif
89}
90
92{
93 switch (type) {
95 state.origin_input_filter = flag;
96 break;
97 case STATE_INPUT_POS:
98 state.pos_input_filter = flag;
99 break;
101 state.speed_input_filter = flag;
102 break;
104 state.accel_input_filter = flag;
105 break;
107 state.attitude_input_filter = flag;
108 break;
110 state.rates_input_filter = flag;
111 break;
113 state.wind_air_input_filter = flag;
114 break;
115 default:
116 break; // nothing to do, wrong type
117 }
118}
119
120/*******************************************************************************
121 * *
122 * transformation functions for the POSITION representations *
123 * *
124 ******************************************************************************/
127
128
131{
132 struct LlaCoor_i lla_i = {0};
133 if (state.ned_initialized_i) {
134 return state.ned_origin_i.lla;
135 } else if (state.ned_initialized_f) {
136 LLA_BFP_OF_REAL(lla_i, state.ned_origin_f.lla);
137 return lla_i;
138 } else if (state.utm_initialized_f) {
139 struct LlaCoor_f lla_f;
140 lla_of_utm_f(&lla_f, &state.utm_origin_f);
141 LLA_BFP_OF_REAL(lla_i, lla_f);
142 return lla_i;
143 } else {
144 return lla_i;
145 }
146}
147
150{
151 struct LlaCoor_f lla_f = {0};
152 if (state.ned_initialized_f) {
153 return state.ned_origin_f.lla;
154 } else if (state.ned_initialized_i) {
155 LLA_FLOAT_OF_BFP(lla_f, state.ned_origin_i.lla);
156 return lla_f;
157 } else if (state.utm_initialized_f) {
158 lla_of_utm_f(&lla_f, &state.utm_origin_f);
159 return lla_f;
160 } else {
161 return lla_f;
162 }
163}
164
167{
168 struct EcefCoor_i ecef_i = {0};
169 if (state.ned_initialized_i) {
170 return state.ned_origin_i.ecef;
171 } else if (state.ned_initialized_f) {
172 ECEF_BFP_OF_REAL(ecef_i, state.ned_origin_f.ecef);
173 return ecef_i;
174 } else {
175 // UTM case is not supported
176 return ecef_i;
177 }
178}
179
182{
183 struct EcefCoor_f ecef_f = {0};
184 if (state.ned_initialized_f) {
185 return state.ned_origin_f.ecef;
186 } else if (state.ned_initialized_i) {
187 ECEF_FLOAT_OF_BFP(ecef_f, state.ned_origin_i.ecef);
188 return ecef_f;
189 } else {
190 // UTM case is not supported
191 return ecef_f;
192 }
193}
194
197{
198 if (state.ned_initialized_i) {
199 return state.ned_origin_i.hmsl;
200 } else if (state.ned_initialized_f) {
201 return (int32_t) MM_OF_M(state.ned_origin_f.hmsl);
202 } else if (state.utm_initialized_f) {
203 return (int32_t) MM_OF_M(state.utm_origin_f.alt);
204 } else {
205 return 0;
206 }
207}
208
211{
212 if (state.ned_initialized_f) {
213 return state.ned_origin_f.hmsl;
214 } else if (state.ned_initialized_i) {
215 return (float) M_OF_MM(state.ned_origin_i.hmsl);
216 } else if (state.utm_initialized_f) {
217 return state.utm_origin_f.alt;
218 } else {
219 return 0.f;
220 }
221}
222
224{
225 if (bit_is_set(state.pos_status, POS_ECEF_I)) {
226 return;
227 }
228
229 if (bit_is_set(state.pos_status, POS_ECEF_F)) {
230 ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
231 } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
232 ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
233 } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
234 /* transform ned_f to ecef_f, set status bit, then convert to int */
235 ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
236 SetBit(state.pos_status, POS_ECEF_F);
237 ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
238 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
239 ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i);
240 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
241 /* transform lla_f to ecef_f, set status bit, then convert to int */
242 ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
243 SetBit(state.pos_status, POS_ECEF_F);
244 ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
245 } else {
246 /* could not get this representation, set errno */
247 //struct EcefCoor_i _ecef_zero = {0};
248 //return _ecef_zero;
249 return;
250 }
251 /* set bit to indicate this representation is computed */
252 SetBit(state.pos_status, POS_ECEF_I);
253}
254
256{
257 if (bit_is_set(state.pos_status, POS_NED_I)) {
258 return;
259 }
260
261 int errno = 0;
262 if (state.ned_initialized_i) {
263 if (bit_is_set(state.pos_status, POS_NED_F)) {
264 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
265 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
266 INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
267 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
268 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
269 SetBit(state.pos_status, POS_ENU_I);
270 INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
271 } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
272 ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
273 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
274 /* transform ecef_f -> ned_f, set status bit, then convert to int */
275 ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
276 SetBit(state.pos_status, POS_NED_F);
277 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
278 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
279 /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
280 ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
281 SetBit(state.pos_status, POS_ECEF_F);
282 ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
283 SetBit(state.pos_status, POS_NED_F);
284 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
285 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
286 ned_of_lla_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
287 } else { /* could not get this representation, set errno */
288 errno = 1;
289 }
290 } else if (state.utm_initialized_f) {
291 if (bit_is_set(state.pos_status, POS_NED_F)) {
292 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
293 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
294 INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
295 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
296 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
297 SetBit(state.pos_status, POS_ENU_I);
298 INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
299 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
300 /* transform utm_f -> ned_f -> ned_i, set status bits */
301 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
302 SetBit(state.pos_status, POS_NED_F);
303 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
304 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
305 /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
306 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
307 SetBit(state.pos_status, POS_UTM_F);
308 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
309 SetBit(state.pos_status, POS_NED_F);
310 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
311 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
312 /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
313 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
314 SetBit(state.pos_status, POS_LLA_F);
315 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
316 SetBit(state.pos_status, POS_UTM_F);
317 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
318 SetBit(state.pos_status, POS_NED_F);
319 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
320 } else { /* could not get this representation, set errno */
321 errno = 2;
322 }
323 } else { /* ned coordinate system not initialized, set errno */
324 errno = 3;
325 }
326 if (errno) {
327 //struct NedCoor_i _ned_zero = {0};
328 //return _ned_zero;
329 return;
330 }
331 /* set bit to indicate this representation is computed */
332 SetBit(state.pos_status, POS_NED_I);
333}
334
336{
337 if (bit_is_set(state.pos_status, POS_ENU_I)) {
338 return;
339 }
340
341 int errno = 0;
342 if (state.ned_initialized_i) {
343 if (bit_is_set(state.pos_status, POS_NED_I)) {
344 INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
345 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
346 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
347 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
348 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
349 SetBit(state.pos_status, POS_NED_I);
350 INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
351 } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
352 enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
353 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
354 /* transform ecef_f -> enu_f, set status bit, then convert to int */
355 enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
356 SetBit(state.pos_status, POS_ENU_F);
357 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
358 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
359 /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
360 ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
361 SetBit(state.pos_status, POS_ECEF_F);
362 enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
363 SetBit(state.pos_status, POS_ENU_F);
364 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
365 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
366 enu_of_lla_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
367 } else { /* could not get this representation, set errno */
368 errno = 1;
369 }
370 } else if (state.utm_initialized_f) {
371 if (bit_is_set(state.pos_status, POS_ENU_F)) {
372 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
373 } else if (bit_is_set(state.pos_status, POS_NED_I)) {
374 INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
375 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
376 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
377 SetBit(state.pos_status, POS_NED_I);
378 INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
379 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
380 /* transform utm_f -> enu_f -> enu_i , set status bits */
381 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
382 SetBit(state.pos_status, POS_ENU_F);
383 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
384 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
385 /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
386 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
387 SetBit(state.pos_status, POS_UTM_F);
388 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
389 SetBit(state.pos_status, POS_ENU_F);
390 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
391 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
392 /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
393 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
394 SetBit(state.pos_status, POS_LLA_F);
395 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
396 SetBit(state.pos_status, POS_UTM_F);
397 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
398 SetBit(state.pos_status, POS_ENU_F);
399 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
400 } else { /* could not get this representation, set errno */
401 errno = 2;
402 }
403 } else { /* ned coordinate system not initialized, set errno */
404 errno = 3;
405 }
406 if (errno) {
407 //struct EnuCoor_i _enu_zero = {0};
408 //return _enu_zero;
409 return;
410 }
411 /* set bit to indicate this representation is computed */
412 SetBit(state.pos_status, POS_ENU_I);
413}
414
423{
424 if (bit_is_set(state.pos_status, POS_LLA_I)) {
425 return;
426 }
427
428 int errno = 0;
429 if (bit_is_set(state.pos_status, POS_ECEF_I)) {
430 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
431 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
432 /* transform ecef_f -> ecef_i -> lla_i, set status bits */
433 ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
434 SetBit(state.pos_status, POS_ECEF_I);
435 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
436 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
437 /* transform utm_f -> lla_f -> lla_i, set status bits */
438 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
439 SetBit(state.pos_status, POS_LLA_F);
440 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
441 } else if (state.ned_initialized_i) {
442 if (bit_is_set(state.pos_status, POS_NED_I)) {
443 /* transform ned_i -> ecef_i -> lla_i, set status bits */
444 ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
445 SetBit(state.pos_status, POS_ECEF_I);
446 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
447 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
448 /* transform enu_i -> ecef_i -> lla_i, set status bits */
449 ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
450 SetBit(state.pos_status, POS_ECEF_I);
451 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
452 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
453 /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
454 NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
455 SetBit(state.pos_status, POS_NED_I);
456 ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
457 SetBit(state.pos_status, POS_ECEF_I);
458 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
459 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
460 /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
461 ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
462 SetBit(state.pos_status, POS_ENU_I);
463 ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
464 SetBit(state.pos_status, POS_ECEF_I);
465 lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
466 } else { /* could not get this representation, set errno */
467 errno = 1;
468 }
469 } else if (state.utm_initialized_f) {
470 if (bit_is_set(state.pos_status, POS_NED_I)) {
471 /* transform ned_i -> ned_f -> utm_f -> lla_f -> lla_i, set status bits */
472 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
473 SetBit(state.pos_status, POS_NED_F);
474 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
475 SetBit(state.pos_status, POS_UTM_F);
476 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
477 SetBit(state.pos_status, POS_LLA_F);
478 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
479 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
480 /* transform enu_i -> enu_f -> utm_f -> lla_f -> lla_i, set status bits */
481 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
482 SetBit(state.pos_status, POS_ENU_F);
483 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
484 SetBit(state.pos_status, POS_UTM_F);
485 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
486 SetBit(state.pos_status, POS_LLA_F);
487 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
488 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
489 /* transform ned_f -> utm_f -> lla_f -> lla_i, set status bits */
490 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
491 SetBit(state.pos_status, POS_UTM_F);
492 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
493 SetBit(state.pos_status, POS_LLA_F);
494 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
495 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
496 /* transform enu_f -> utm_f -> lla_f -> lla_i, set status bits */
497 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
498 SetBit(state.pos_status, POS_UTM_F);
499 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
500 SetBit(state.pos_status, POS_LLA_F);
501 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
502 } else { /* could not get this representation, set errno */
503 errno = 2;
504 }
505 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
506 LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
507 } else { /* ned coordinate system not initialized, set errno */
508 errno = 3;
509 }
510 if (errno) {
511 /* could not get this representation, set errno */
512 //struct LlaCoor_i _lla_zero = {0};
513 //return _lla_zero;
514 return;
515 }
516 /* set bit to indicate this representation is computed */
517 SetBit(state.pos_status, POS_LLA_I);
518}
519
521{
522 if (bit_is_set(state.pos_status, POS_UTM_F)) {
523 return;
524 }
525
526 if (bit_is_set(state.pos_status, POS_LLA_F)) {
527 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
528 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
529 /* transform lla_i -> lla_f -> utm_f, set status bits */
530 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
531 SetBit(state.pos_status, POS_LLA_F);
532 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
533 } else if (state.utm_initialized_f) {
534 if (bit_is_set(state.pos_status, POS_ENU_F)) {
535 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
536 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
537 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
538 SetBit(state.pos_status, POS_ENU_F);
539 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
540 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
541 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
542 } else if (bit_is_set(state.pos_status, POS_NED_I)) {
543 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
544 SetBit(state.pos_status, POS_NED_F);
545 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
546 }
547 } else {
548 /* could not get this representation, set errno */
549 //struct EcefCoor_f _ecef_zero = {0.0f};
550 //return _ecef_zero;
551 return;
552 }
553 /* set bit to indicate this representation is computed */
554 SetBit(state.pos_status, POS_UTM_F);
555}
556
558{
559 if (bit_is_set(state.pos_status, POS_ECEF_F)) {
560 return;
561 }
562
563 if (bit_is_set(state.pos_status, POS_ECEF_I)) {
564 ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
565 } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
566 ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
567 } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
568 /* transform ned_i -> ecef_i -> ecef_f, set status bits */
569 ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
570 SetBit(state.pos_status, POS_ECEF_F);
571 ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
572 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
573 ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
574 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
575 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
576 SetBit(state.pos_status, POS_LLA_F);
577 ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
578 } else {
579 /* could not get this representation, set errno */
580 //struct EcefCoor_f _ecef_zero = {0.0f};
581 //return _ecef_zero;
582 return;
583 }
584 /* set bit to indicate this representation is computed */
585 SetBit(state.pos_status, POS_ECEF_F);
586}
587
589{
590 if (bit_is_set(state.pos_status, POS_NED_F)) {
591 return;
592 }
593
594 int errno = 0;
595 if (state.ned_initialized_f) {
596 if (bit_is_set(state.pos_status, POS_NED_I)) {
597 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
598 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
599 ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
600 } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
601 /* transform ecef_i -> ned_i -> ned_f, set status bits */
602 ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
603 SetBit(state.pos_status, POS_NED_I);
604 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
605 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
606 ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f);
607 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
608 /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
609 ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
610 SetBit(state.pos_status, POS_ECEF_I);
611 ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
612 SetBit(state.pos_status, POS_NED_I);
613 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
614 } else { /* could not get this representation, set errno */
615 errno = 1;
616 }
617 } else if (state.utm_initialized_f) {
618 if (bit_is_set(state.pos_status, POS_NED_I)) {
619 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
620 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
621 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
622 SetBit(state.pos_status, POS_ENU_F);
623 VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
624 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
625 VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
626 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
627 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
628 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
629 /* transform lla_f -> utm_f -> ned, set status bits */
630 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
631 SetBit(state.pos_status, POS_UTM_F);
632 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
633 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
634 /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
635 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
636 SetBit(state.pos_status, POS_LLA_F);
637 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
638 SetBit(state.pos_status, POS_UTM_F);
639 NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
640 } else { /* could not get this representation, set errno */
641 errno = 2;
642 }
643 } else { /* ned coordinate system not initialized, set errno */
644 errno = 3;
645 }
646 if (errno) {
647 //struct NedCoor_f _ned_zero = {0.0f};
648 //return _ned_zero;
649 return;
650 }
651 /* set bit to indicate this representation is computed */
652 SetBit(state.pos_status, POS_NED_F);
653}
654
656{
657 if (bit_is_set(state.pos_status, POS_ENU_F)) {
658 return;
659 }
660
661 int errno = 0;
662 if (state.ned_initialized_f) {
663 if (bit_is_set(state.pos_status, POS_NED_F)) {
664 VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
665 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
666 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
667 } else if (bit_is_set(state.pos_status, POS_NED_I)) {
668 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
669 SetBit(state.pos_status, POS_NED_F);
670 VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
671 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
672 enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
673 } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
674 /* transform ecef_i -> enu_i -> enu_f, set status bits */
675 enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
676 SetBit(state.pos_status, POS_ENU_I);
677 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
678 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
679 enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
680 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
681 /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
682 ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
683 SetBit(state.pos_status, POS_ECEF_I);
684 enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
685 SetBit(state.pos_status, POS_ENU_I);
686 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
687 } else { /* could not get this representation, set errno */
688 errno = 1;
689 }
690 } else if (state.utm_initialized_f) {
691 if (bit_is_set(state.pos_status, POS_ENU_I)) {
692 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
693 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
694 VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
695 } else if (bit_is_set(state.pos_status, POS_NED_I)) {
696 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
697 SetBit(state.pos_status, POS_NED_F);
698 VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
699 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
700 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
701 } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
702 /* transform lla_f -> utm_f -> enu, set status bits */
703 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
704 SetBit(state.pos_status, POS_UTM_F);
705 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
706 } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
707 /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
708 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
709 SetBit(state.pos_status, POS_LLA_F);
710 utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
711 SetBit(state.pos_status, POS_UTM_F);
712 ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
713 } else { /* could not get this representation, set errno */
714 errno = 2;
715 }
716 } else { /* ned coordinate system not initialized, set errno */
717 errno = 3;
718 }
719 if (errno) {
720 //struct EnuCoor_f _enu_zero = {0.0f};
721 //return _enu_zero;
722 return;
723 }
724 /* set bit to indicate this representation is computed */
725 SetBit(state.pos_status, POS_ENU_F);
726}
727
729{
730 if (bit_is_set(state.pos_status, POS_LLA_F)) {
731 return;
732 }
733
734 int errno = 0;
735 if (bit_is_set(state.pos_status, POS_LLA_I)) {
736 LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
737 } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
738 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
739 } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
740 /* transform ecef_i -> ecef_f -> lla_f, set status bits */
741 ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
742 SetBit(state.pos_status, POS_ECEF_F);
743 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
744 } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
745 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
746 } else if (state.ned_initialized_f) {
747 if (bit_is_set(state.pos_status, POS_NED_F)) {
748 /* transform ned_f -> ecef_f -> lla_f, set status bits */
749 ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
750 SetBit(state.pos_status, POS_ECEF_F);
751 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
752 } else if (bit_is_set(state.pos_status, POS_NED_I)) {
753 /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
754 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
755 SetBit(state.pos_status, POS_NED_F);
756 ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
757 SetBit(state.pos_status, POS_ECEF_F);
758 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
759 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
760 /* transform enu_f -> ecef_f -> lla_f, set status bits */
761 ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
762 SetBit(state.pos_status, POS_ECEF_F);
763 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
764 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
765 /* transform enu_i -> enu_f -> ecef_f -> lla_f, set status bits */
766 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
767 SetBit(state.pos_status, POS_ENU_F);
768 ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
769 SetBit(state.pos_status, POS_ECEF_F);
770 lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
771 } else { /* could not get this representation, set errno */
772 errno = 1;
773 }
774 } else if (state.utm_initialized_f) {
775 if (bit_is_set(state.pos_status, POS_NED_I)) {
776 /* transform ned_i -> ned_f -> utm_f -> lla_f, set status bits */
777 NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
778 SetBit(state.pos_status, POS_NED_F);
779 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
780 SetBit(state.pos_status, POS_UTM_F);
781 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
782 } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
783 /* transform enu_i -> enu_f -> utm_f -> lla_f, set status bits */
784 ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
785 SetBit(state.pos_status, POS_ENU_F);
786 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
787 SetBit(state.pos_status, POS_UTM_F);
788 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
789 } else if (bit_is_set(state.pos_status, POS_NED_F)) {
790 /* transform ned_f -> utm_f -> lla_f, set status bits */
791 UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
792 SetBit(state.pos_status, POS_UTM_F);
793 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
794 } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
795 /* transform enu_f -> utm_f -> lla_f, set status bits */
796 UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
797 SetBit(state.pos_status, POS_UTM_F);
798 lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
799 } else { /* could not get this representation, set errno */
800 errno = 2;
801 }
802 } else { /* ned coordinate system not initialized, set errno */
803 errno = 3;
804 }
805 if (errno) {
806 /* could not get this representation, set errno */
807 //struct LlaCoor_f _lla_zero = {0.0};
808 //return _lla_zero;
809 return;
810 }
811 /* set bit to indicate this representation is computed */
812 SetBit(state.pos_status, POS_LLA_F);
813}
814
815
816
817
818
819
820/******************************************************************************
821 * *
822 * Transformation functions for the SPEED representations *
823 * *
824 *****************************************************************************/
827/************************ Set functions ****************************/
828
830{
831 if (bit_is_set(state.speed_status, SPEED_NED_I)) {
832 return;
833 }
834
835 int errno = 0;
836 if (state.ned_initialized_i) {
837 if (bit_is_set(state.speed_status, SPEED_NED_F)) {
838 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
839 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
840 INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
841 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
842 SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
843 SetBit(state.speed_status, SPEED_ENU_I);
844 INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
845 } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
846 ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
847 } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
848 /* transform ecef_f -> ecef_i -> ned_i , set status bits */
849 SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
850 SetBit(state.speed_status, SPEED_ECEF_I);
851 ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
852 } else { /* could not get this representation, set errno */
853 errno = 1;
854 }
855 } else if (state.utm_initialized_f) {
856 if (bit_is_set(state.speed_status, SPEED_NED_F)) {
857 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
858 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
859 INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
860 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
861 SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
862 SetBit(state.speed_status, SPEED_ENU_I);
863 INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
864 } else { /* could not get this representation, set errno */
865 errno = 2;
866 }
867 } else { /* ned coordinate system not initialized, set errno */
868 errno = 3;
869 }
870 if (errno) {
871 //struct NedCoor_i _ned_zero = {0};
872 //return _ned_zero;
873 return;
874 }
875 /* set bit to indicate this representation is computed */
876 SetBit(state.speed_status, SPEED_NED_I);
877}
878
880{
881 if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
882 return;
883 }
884
885 int errno = 0;
886 if (state.ned_initialized_i) {
887 if (bit_is_set(state.speed_status, SPEED_NED_I)) {
888 INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
889 }
890 if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
891 SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
892 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
893 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
894 SetBit(state.speed_status, SPEED_NED_I);
895 INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
896 } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
897 enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
898 } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
899 /* transform ecef_f -> ecef_i -> enu_i , set status bits */
900 SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
901 SetBit(state.speed_status, SPEED_ECEF_I);
902 enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
903 } else { /* could not get this representation, set errno */
904 errno = 1;
905 }
906 } else if (state.utm_initialized_f) {
907 if (bit_is_set(state.speed_status, SPEED_NED_I)) {
908 INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
909 }
910 if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
911 SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
912 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
913 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
914 SetBit(state.speed_status, SPEED_NED_I);
915 INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
916 } else { /* could not get this representation, set errno */
917 errno = 2;
918 }
919 } else { /* ned coordinate system not initialized, set errno */
920 errno = 3;
921 }
922 if (errno) {
923 //struct EnuCoor_i _enu_zero = {0};
924 //return _enu_zero;
925 return;
926 }
927 /* set bit to indicate this representation is computed */
928 SetBit(state.speed_status, SPEED_ENU_I);
929}
930
932{
933 if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
934 return;
935 }
936
937 if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
938 SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
939 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
940 ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
941 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
942 /* transform ned_f -> ned_i -> ecef_i , set status bits */
943 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
944 SetBit(state.speed_status, SPEED_NED_I);
945 ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
946 } else {
947 /* could not get this representation, set errno */
948 //struct EcefCoor_i _ecef_zero = {0};
949 //return _ecef_zero;
950 return;
951 }
952 /* set bit to indicate this representation is computed */
953 SetBit(state.speed_status, SPEED_ECEF_I);
954}
955
957{
958 if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
959 return;
960 }
961
962 if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
963 state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
964 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
965 uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
966 state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
967 state.h_speed_norm_i = int32_sqrt(n2);
968 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
969 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
970 SetBit(state.speed_status, SPEED_HNORM_F);
971 state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
972 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
973 uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x +
974 state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC;
975 state.h_speed_norm_i = int32_sqrt(n2);
976 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
977 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
978 SetBit(state.speed_status, SPEED_HNORM_F);
979 state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
980 } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
981 /* transform ecef speed to ned, set status bit, then compute norm */
982 ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
983 SetBit(state.speed_status, SPEED_NED_I);
984 uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
985 state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
986 state.h_speed_norm_i = int32_sqrt(n2);
987 } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
988 ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
989 SetBit(state.speed_status, SPEED_NED_F);
990 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
991 SetBit(state.speed_status, SPEED_HNORM_F);
992 state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
993 } else {
994 //int32_t _norm_zero = 0;
995 //return _norm_zero;
996 return;
997 }
998 /* set bit to indicate this representation is computed */
999 SetBit(state.speed_status, SPEED_HNORM_I);
1000}
1001
1003{
1004 if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1005 return;
1006 }
1007
1008 if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1009 state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
1010 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1011 state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
1012 INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
1013 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1014 state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
1015 INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
1016 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1017 SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
1018 SetBit(state.speed_status, SPEED_NED_I);
1019 state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
1020 INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
1021 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1022 SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
1023 SetBit(state.speed_status, SPEED_ENU_I);
1024 state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
1025 INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
1026 } else {
1027 return;
1028 }
1029 /* set bit to indicate this representation is computed */
1030 SetBit(state.speed_status, SPEED_HDIR_I);
1031}
1032
1034{
1035 if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1036 return;
1037 }
1038
1039 int errno = 0;
1040 if (state.ned_initialized_f) {
1041 if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1042 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1043 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1044 VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
1045 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1046 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1047 SetBit(state.speed_status, SPEED_ENU_F);
1048 VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
1049 } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1050 ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
1051 } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1052 /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1053 SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
1054 SetBit(state.speed_status, SPEED_ECEF_F);
1055 ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
1056 } else { /* could not get this representation, set errno */
1057 errno = 1;
1058 }
1059 } else if (state.utm_initialized_f) {
1060 if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1061 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1062 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1063 VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
1064 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1065 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1066 SetBit(state.speed_status, SPEED_ENU_F);
1067 VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
1068 } else { /* could not get this representation, set errno */
1069 errno = 2;
1070 }
1071 } else { /* ned coordinate system not initialized, set errno */
1072 errno = 3;
1073 }
1074 if (errno) {
1075 //struct NedCoor_f _ned_zero = {0.0f};
1076 //return _ned_zero;
1077 return;
1078 }
1079 /* set bit to indicate this representation is computed */
1080 SetBit(state.speed_status, SPEED_NED_F);
1081}
1082
1084{
1085 if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1086 return;
1087 }
1088
1089 int errno = 0;
1090 if (state.ned_initialized_f) {
1091 if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1092 VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
1093 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1094 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1095 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1096 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1097 SetBit(state.speed_status, SPEED_NED_F);
1098 VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
1099 } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1100 enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
1101 } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1102 /* transform ecef_I -> ecef_f -> enu_f , set status bits */
1103 SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
1104 SetBit(state.speed_status, SPEED_ECEF_F);
1105 enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
1106 } else { /* could not get this representation, set errno */
1107 errno = 1;
1108 }
1109 } else if (state.utm_initialized_f) {
1110 if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1111 VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
1112 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1113 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1114 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1115 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1116 SetBit(state.speed_status, SPEED_NED_F);
1117 VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
1118 } else { /* could not get this representation, set errno */
1119 errno = 2;
1120 }
1121 } else { /* ned coordinate system not initialized, set errno */
1122 errno = 3;
1123 }
1124 if (errno) {
1125 //struct EnuCoor_f _enu_zero = {0};
1126 //return _enu_zero;
1127 return;
1128 }
1129 /* set bit to indicate this representation is computed */
1130 SetBit(state.speed_status, SPEED_ENU_F);
1131}
1132
1134{
1135 if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1136 return;
1137 }
1138
1139 if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1140 SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
1141 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1142 ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
1143 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1144 /* transform ned_f -> ned_i -> ecef_i , set status bits */
1145 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1146 SetBit(state.speed_status, SPEED_NED_F);
1147 ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
1148 } else {
1149 /* could not get this representation, set errno */
1150 //struct EcefCoor_f _ecef_zero = {0.0f};
1151 //return _ecef_zero;
1152 return;
1153 }
1154 /* set bit to indicate this representation is computed */
1155 SetBit(state.speed_status, SPEED_ECEF_F);
1156}
1157
1159{
1160 if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
1161 return;
1162 }
1163
1164 if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
1165 state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
1166 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1167 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1168 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1169 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
1170 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1171 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1172 SetBit(state.speed_status, SPEED_NED_F);
1173 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1174 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1175 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1176 SetBit(state.speed_status, SPEED_ENU_F);
1177 state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
1178 } else {
1179 return;
1180 }
1181 /* set bit to indicate this representation is computed */
1182 SetBit(state.speed_status, SPEED_HNORM_F);
1183}
1184
1186{
1187 if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1188 return;
1189 }
1190
1191 if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1192 state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
1193 } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1194 state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1195 } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1196 state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
1197 } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1198 SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1199 SetBit(state.speed_status, SPEED_NED_F);
1200 state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1201 } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1202 SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1203 SetBit(state.speed_status, SPEED_ENU_F);
1204 state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
1205 } else {
1206 return;
1207 }
1208 /* set bit to indicate this representation is computed */
1209 SetBit(state.speed_status, SPEED_HDIR_F);
1210}
1211
1212
1213
1214
1215/******************************************************************************
1216 * *
1217 * Transformation functions for the ACCELERATION representations *
1218 * *
1219 *****************************************************************************/
1222
1224{
1225 if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1226 return;
1227 }
1228
1229 int errno = 0;
1230 if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1231 ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1232 }
1233 else if (state.ned_initialized_i) {
1234 if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1235 ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
1236 } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1237 /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1238 ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1239 SetBit(state.accel_status, ACCEL_ECEF_I);
1240 ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
1241 } else { /* could not get this representation, set errno */
1242 errno = 1;
1243 }
1244 } else { /* ned coordinate system not initialized, set errno */
1245 errno = 2;
1246 }
1247 if (errno) {
1248 //struct NedCoor_i _ned_zero = {0};
1249 //return _ned_zero;
1250 return;
1251 }
1252 /* set bit to indicate this representation is computed */
1253 SetBit(state.accel_status, ACCEL_NED_I);
1254}
1255
1257{
1258 if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1259 return;
1260 }
1261
1262 int errno = 0;
1263 if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1264 ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1265 }
1266 else if (state.ned_initialized_i) {
1267 if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1268 ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1269 } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1270 /* transform ned_f -> ned_i -> ecef_i , set status bits */
1271 ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1272 SetBit(state.accel_status, ACCEL_NED_I);
1273 ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1274 } else {
1275 /* could not get this representation, set errno */
1276 errno = 1;
1277 }
1278 } else { /* ned coordinate system not initialized, set errno */
1279 errno = 2;
1280 }
1281 if (errno) {
1282 return;
1283 }
1284 /* set bit to indicate this representation is computed */
1285 SetBit(state.accel_status, ACCEL_ECEF_I);
1286}
1287
1289{
1290 if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1291 return;
1292 }
1293
1294 int errno = 0;
1295 if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1296 ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1297 }
1298 else if (state.ned_initialized_f) {
1299 if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1300 ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1301 } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1302 /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1303 ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
1304 SetBit(state.accel_status, ACCEL_ECEF_F);
1305 ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1306 } else { /* could not get this representation, set errno */
1307 errno = 1;
1308 }
1309 } else { /* ned coordinate system not initialized, set errno */
1310 errno = 2;
1311 }
1312 if (errno) {
1313 //struct NedCoor_f _ned_zero = {0.0f};
1314 //return _ned_zero;
1315 return;
1316 }
1317 /* set bit to indicate this representation is computed */
1318 SetBit(state.accel_status, ACCEL_NED_F);
1319}
1320
1322{
1323 if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1324 return;
1325 }
1326
1327 int errno = 0;
1328 if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1329 ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
1330 }
1331 else if (state.ned_initialized_f) {
1332 if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1333 ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1334 } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1335 /* transform ned_f -> ned_i -> ecef_i , set status bits */
1336 ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1337 SetBit(state.accel_status, ACCEL_NED_F);
1338 ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1339 } else {
1340 /* could not get this representation, set errno */
1341 errno = 1;
1342 }
1343 } else { /* ned coordinate system not initialized, set errno */
1344 errno = 2;
1345 }
1346 if (errno) {
1347 return;
1348 }
1349 /* set bit to indicate this representation is computed */
1350 SetBit(state.accel_status, ACCEL_ECEF_F);
1351}
1352
1353
1354/******************************************************************************
1355 * *
1356 * Transformation functions for the ANGULAR RATE representations *
1357 * *
1358 *****************************************************************************/
1361
1363{
1364 if (bit_is_set(state.rate_status, RATE_I)) {
1365 return;
1366 }
1367
1368 if (bit_is_set(state.rate_status, RATE_F)) {
1369 RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
1370 } else {
1371 return;
1372 }
1373 /* set bit to indicate this representation is computed */
1374 SetBit(state.rate_status, RATE_I);
1375}
1376
1378{
1379 if (bit_is_set(state.rate_status, RATE_F)) {
1380 return;
1381 }
1382
1383 if (bit_is_set(state.rate_status, RATE_I)) {
1384 RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
1385 } else {
1386 return;
1387 }
1388 /* set bit to indicate this representation is computed */
1389 SetBit(state.rate_status, RATE_F);
1390}
1391
1393
1394
1395/******************************************************************************
1396 * *
1397 * Transformation functions for the WIND- AND AIRSPEED representations *
1398 * *
1399 *****************************************************************************/
1402
1404{
1405 if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1406 return;
1407 }
1408
1409 if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1410 state.windspeed_i.vect2.x = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.x);
1411 state.windspeed_i.vect2.y = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.y);
1412 } else {
1413 return;
1414 }
1415 /* set bit to indicate this representation is computed */
1416 SetBit(state.wind_air_status , WINDSPEED_I);
1417}
1418
1420{
1421 if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1422 return;
1423 }
1424
1425 if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1426 state.windspeed_i.vect3.z = SPEED_BFP_OF_REAL(state.windspeed_f.vect3.z);
1427 } else {
1428 return;
1429 }
1430 /* set bit to indicate this representation is computed */
1431 SetBit(state.wind_air_status, DOWNWIND_I);
1432}
1433
1435{
1436 if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1437 return;
1438 }
1439
1440 if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1441 state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f);
1442 } else {
1443 return;
1444 }
1445 /* set bit to indicate this representation is computed */
1446 SetBit(state.wind_air_status, AIRSPEED_I);
1447}
1448
1450{
1451 if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1452 return;
1453 }
1454
1455 if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1456 state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.x);
1457 state.windspeed_f.vect2.y = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.y);
1458 } else {
1459 return;
1460 }
1461 /* set bit to indicate this representation is computed */
1462 SetBit(state.wind_air_status, WINDSPEED_F);
1463}
1464
1466{
1467 if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1468 return;
1469 }
1470
1471 if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1472 state.windspeed_f.vect3.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect3.z);
1473 } else {
1474 return;
1475 }
1476 /* set bit to indicate this representation is computed */
1477 SetBit(state.wind_air_status, DOWNWIND_F);
1478}
1479
1481{
1482 if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1483 return;
1484 }
1485
1486 if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1487 state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i);
1488 } else {
1489 return;
1490 }
1491 /* set bit to indicate this representation is computed */
1492 SetBit(state.wind_air_status, AIRSPEED_F);
1493}
1494
1495
if(GpsFixValid() &&e_identification_started)
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_SPEED
Definition state.h:142
#define STATE_INPUT_POS
Definition state.h:141
#define STATE_INPUT_ACCEL
Definition state.h:143
#define STATE_INPUT_ORIGIN
Definition state.h:140
#define STATE_INPUT_ANY
default
Definition state.h:139
#define STATE_INPUT_WIND_AIR
Definition state.h:146
#define STATE_INPUT_ATTITUDE
Definition state.h:144
#define FLOAT_VECT2_NORM(_v)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
uint32_t int32_sqrt(uint32_t in)
#define INT32_SPEED_FRAC
#define INT32_COURSE_NORMALIZE(_a)
#define SPEED_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
#define ECEF_FLOAT_OF_BFP(_o, _i)
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
#define ENU_FLOAT_OF_BFP(_o, _i)
#define ECEF_BFP_OF_REAL(_o, _i)
#define ENU_BFP_OF_REAL(_o, _i)
#define LLA_BFP_OF_REAL(_o, _i)
#define NED_BFP_OF_REAL(_o, _i)
void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local NED.
#define NED_FLOAT_OF_BFP(_o, _i)
void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local ENU.
#define INT32_VECT3_ENU_OF_NED(_o, _i)
#define VECT3_ENU_OF_NED(_o, _i)
void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a local ENU position to ECEF.
#define VECT3_NED_OF_ENU(_o, _i)
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
#define MM_OF_M(_m)
#define LLA_FLOAT_OF_BFP(_o, _i)
#define INT32_VECT3_NED_OF_ENU(_o, _i)
#define M_OF_MM(_mm)
void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to ENU.
void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Convert a local NED position to ECEF.
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
#define ACCEL_ECEF_I
Definition state.h:106
#define ACCEL_NED_F
Definition state.h:109
void stateCalcAccelNed_i(void)
Definition state.c:1223
void stateCalcAccelEcef_i(void)
Definition state.c:1256
#define ACCEL_ECEF_F
Definition state.h:108
void stateCalcAccelEcef_f(void)
Definition state.c:1321
#define ACCEL_NED_I
Definition state.h:107
void stateCalcAccelNed_f(void)
Definition state.c:1288
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition state.h:1243
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
struct State state
Definition state.c:36
void stateInit(void)
Definition state.c:57
Structure holding vehicle state data.
Definition state.h:152
#define POS_LLA_F
Definition state.h:79
#define POS_UTM_F
Definition state.h:80
#define POS_LLA_I
Definition state.h:74
#define POS_ENU_F
Definition state.h:78
#define POS_ECEF_F
Definition state.h:76
void stateCalcPositionNed_f(void)
Definition state.c:588
struct EcefCoor_f stateGetEcefOrigin_f(void)
Get the ECEF position of the frame origin (float).
Definition state.c:181
void stateCalcPositionUtm_f(void)
Definition state.c:520
void stateCalcPositionEnu_f(void)
Definition state.c:655
#define POS_ENU_I
Definition state.h:73
#define POS_ECEF_I
Definition state.h:71
void stateCalcPositionEcef_f(void)
Definition state.c:557
#define POS_NED_F
Definition state.h:77
void stateCalcPositionLla_f(void)
Definition state.c:728
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
Definition state.c:196
void stateCalcPositionEcef_i(void)
Definition state.c:223
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
void stateCalcPositionNed_i(void)
Definition state.c:255
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition state.c:422
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
#define POS_NED_I
Definition state.h:72
struct EcefCoor_i stateGetEcefOrigin_i(void)
Get the ECEF position of the frame origin (int).
Definition state.c:166
void stateCalcPositionEnu_i(void)
Definition state.c:335
void stateCalcBodyRates_f(void)
Definition state.c:1377
#define RATE_F
Definition state.h:117
#define RATE_I
Definition state.h:116
void stateCalcBodyRates_i(void)
Definition state.c:1362
#define SPEED_ENU_F
Definition state.h:96
#define SPEED_ENU_I
Definition state.h:91
void stateCalcSpeedEnu_f(void)
Definition state.c:1083
#define SPEED_ECEF_F
Definition state.h:94
void stateCalcSpeedNed_f(void)
Definition state.c:1033
#define SPEED_ECEF_I
Definition state.h:89
void stateCalcHorizontalSpeedDir_i(void)
Definition state.c:1002
#define SPEED_NED_F
Definition state.h:95
void stateCalcHorizontalSpeedNorm_f(void)
Definition state.c:1158
void stateCalcHorizontalSpeedNorm_i(void)
Definition state.c:956
void stateCalcSpeedEcef_f(void)
Definition state.c:1133
#define SPEED_HDIR_I
Definition state.h:93
void stateCalcSpeedEnu_i(void)
Definition state.c:879
#define SPEED_HNORM_F
Definition state.h:97
#define SPEED_NED_I
Definition state.h:90
void stateCalcHorizontalSpeedDir_f(void)
Definition state.c:1185
void stateCalcSpeedNed_i(void)
Definition state.c:829
#define SPEED_HNORM_I
Definition state.h:92
#define SPEED_HDIR_F
Definition state.h:98
void stateCalcSpeedEcef_i(void)
Definition state.c:931
#define WINDSPEED_I
Definition state.h:124
#define DOWNWIND_F
Definition state.h:128
#define WINDSPEED_F
Definition state.h:127
#define AIRSPEED_I
Definition state.h:126
void stateCalcAirspeed_i(void)
Definition state.c:1434
void stateCalcVerticalWindspeed_i(void)
Definition state.c:1419
void stateCalcAirspeed_f(void)
Definition state.c:1480
#define AIRSPEED_F
Definition state.h:129
void stateCalcHorizontalWindspeed_f(void)
Definition state.c:1449
void stateCalcHorizontalWindspeed_i(void)
Definition state.c:1403
#define DOWNWIND_I
Definition state.h:125
void stateCalcVerticalWindspeed_f(void)
Definition state.c:1465
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
int32_t int32_atan2(int32_t y, int32_t x)
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
API to get/set the generic vehicle states.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.