Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
34 #include "state.h"
35 
36 struct State state;
37 
38 #if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
39 /* Preflight checks */
41 static struct preflight_check_t state_pfc;
42 
43 static void state_preflight(struct preflight_result_t *result) {
44  if(!stateIsAttitudeValid()) {
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 
57 void stateInit(void)
58 {
59  state.pos_status = 0;
60  state.speed_status = 0;
61  state.accel_status = 0;
63  state.rate_status = 0;
65  state.ned_initialized_i = false;
66  state.ned_initialized_f = false;
67  state.utm_initialized_f = false;
68 
69  /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
71 
72  /* Register preflight checks */
73 #if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
74  preflight_check_register(&state_pfc, state_preflight);
75 #endif
76 }
77 
78 
79 /*******************************************************************************
80  * *
81  * transformation functions for the POSITION representations *
82  * *
83  ******************************************************************************/
88 {
89  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
90  return;
91  }
92 
93  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
95  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
97  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
98  /* transform ned_f to ecef_f, set status bit, then convert to int */
100  SetBit(state.pos_status, POS_ECEF_F);
102  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
104  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
105  /* transform lla_f to ecef_f, set status bit, then convert to int */
107  SetBit(state.pos_status, POS_ECEF_F);
109  } else {
110  /* could not get this representation, set errno */
111  //struct EcefCoor_i _ecef_zero = {0};
112  //return _ecef_zero;
113  return;
114  }
115  /* set bit to indicate this representation is computed */
116  SetBit(state.pos_status, POS_ECEF_I);
117 }
118 
120 {
121  if (bit_is_set(state.pos_status, POS_NED_I)) {
122  return;
123  }
124 
125  int errno = 0;
126  if (state.ned_initialized_i) {
127  if (bit_is_set(state.pos_status, POS_NED_F)) {
129  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
131  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
133  SetBit(state.pos_status, POS_ENU_I);
135  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
137  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
138  /* transform ecef_f -> ned_f, set status bit, then convert to int */
140  SetBit(state.pos_status, POS_NED_F);
142  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
143  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
145  SetBit(state.pos_status, POS_ECEF_F);
147  SetBit(state.pos_status, POS_NED_F);
149  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
151  } else { /* could not get this representation, set errno */
152  errno = 1;
153  }
154  } else if (state.utm_initialized_f) {
155  if (bit_is_set(state.pos_status, POS_NED_F)) {
157  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
159  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
161  SetBit(state.pos_status, POS_ENU_I);
163  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
164  /* transform utm_f -> ned_f -> ned_i, set status bits */
166  SetBit(state.pos_status, POS_NED_F);
168  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
169  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
171  SetBit(state.pos_status, POS_UTM_F);
173  SetBit(state.pos_status, POS_NED_F);
175  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
176  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
178  SetBit(state.pos_status, POS_LLA_F);
180  SetBit(state.pos_status, POS_UTM_F);
182  SetBit(state.pos_status, POS_NED_F);
184  } else { /* could not get this representation, set errno */
185  errno = 2;
186  }
187  } else { /* ned coordinate system not initialized, set errno */
188  errno = 3;
189  }
190  if (errno) {
191  //struct NedCoor_i _ned_zero = {0};
192  //return _ned_zero;
193  return;
194  }
195  /* set bit to indicate this representation is computed */
196  SetBit(state.pos_status, POS_NED_I);
197 }
198 
200 {
201  if (bit_is_set(state.pos_status, POS_ENU_I)) {
202  return;
203  }
204 
205  int errno = 0;
206  if (state.ned_initialized_i) {
207  if (bit_is_set(state.pos_status, POS_NED_I)) {
209  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
211  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
213  SetBit(state.pos_status, POS_NED_I);
215  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
217  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
218  /* transform ecef_f -> enu_f, set status bit, then convert to int */
220  SetBit(state.pos_status, POS_ENU_F);
222  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
223  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
225  SetBit(state.pos_status, POS_ECEF_F);
227  SetBit(state.pos_status, POS_ENU_F);
229  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
231  } else { /* could not get this representation, set errno */
232  errno = 1;
233  }
234  } else if (state.utm_initialized_f) {
235  if (bit_is_set(state.pos_status, POS_ENU_F)) {
237  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
239  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
241  SetBit(state.pos_status, POS_NED_I);
243  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
244  /* transform utm_f -> enu_f -> enu_i , set status bits */
246  SetBit(state.pos_status, POS_ENU_F);
248  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
249  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
251  SetBit(state.pos_status, POS_UTM_F);
253  SetBit(state.pos_status, POS_ENU_F);
255  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
256  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
258  SetBit(state.pos_status, POS_LLA_F);
260  SetBit(state.pos_status, POS_UTM_F);
262  SetBit(state.pos_status, POS_ENU_F);
264  } else { /* could not get this representation, set errno */
265  errno = 2;
266  }
267  } else { /* ned coordinate system not initialized, set errno */
268  errno = 3;
269  }
270  if (errno) {
271  //struct EnuCoor_i _enu_zero = {0};
272  //return _enu_zero;
273  return;
274  }
275  /* set bit to indicate this representation is computed */
276  SetBit(state.pos_status, POS_ENU_I);
277 }
278 
287 {
288  if (bit_is_set(state.pos_status, POS_LLA_I)) {
289  return;
290  }
291 
292  int errno = 0;
293  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
295  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
296  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
298  SetBit(state.pos_status, POS_ECEF_I);
300  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
301  /* transform utm_f -> lla_f -> lla_i, set status bits */
303  SetBit(state.pos_status, POS_LLA_F);
305  } else if (state.ned_initialized_i) {
306  if (bit_is_set(state.pos_status, POS_NED_I)) {
307  /* transform ned_i -> ecef_i -> lla_i, set status bits */
309  SetBit(state.pos_status, POS_ECEF_I);
311  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
312  /* transform enu_i -> ecef_i -> lla_i, set status bits */
314  SetBit(state.pos_status, POS_ECEF_I);
316  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
317  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
319  SetBit(state.pos_status, POS_NED_I);
321  SetBit(state.pos_status, POS_ECEF_I);
323  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
324  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
326  SetBit(state.pos_status, POS_ENU_I);
328  SetBit(state.pos_status, POS_ECEF_I);
330  } else { /* could not get this representation, set errno */
331  errno = 1;
332  }
333  } else if (state.utm_initialized_f) {
334  if (bit_is_set(state.pos_status, POS_NED_I)) {
335  /* transform ned_i -> ned_f -> utm_f -> lla_f -> lla_i, set status bits */
337  SetBit(state.pos_status, POS_NED_F);
339  SetBit(state.pos_status, POS_UTM_F);
341  SetBit(state.pos_status, POS_LLA_F);
343  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
344  /* transform enu_i -> enu_f -> utm_f -> lla_f -> lla_i, set status bits */
346  SetBit(state.pos_status, POS_ENU_F);
348  SetBit(state.pos_status, POS_UTM_F);
350  SetBit(state.pos_status, POS_LLA_F);
352  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
353  /* transform ned_f -> utm_f -> lla_f -> lla_i, set status bits */
355  SetBit(state.pos_status, POS_UTM_F);
357  SetBit(state.pos_status, POS_LLA_F);
359  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
360  /* transform enu_f -> utm_f -> lla_f -> lla_i, set status bits */
362  SetBit(state.pos_status, POS_UTM_F);
364  SetBit(state.pos_status, POS_LLA_F);
366  } else { /* could not get this representation, set errno */
367  errno = 2;
368  }
369  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
371  } else { /* ned coordinate system not initialized, set errno */
372  errno = 3;
373  }
374  if (errno) {
375  /* could not get this representation, set errno */
376  //struct LlaCoor_i _lla_zero = {0};
377  //return _lla_zero;
378  return;
379  }
380  /* set bit to indicate this representation is computed */
381  SetBit(state.pos_status, POS_LLA_I);
382 }
383 
385 {
386  if (bit_is_set(state.pos_status, POS_UTM_F)) {
387  return;
388  }
389 
390  if (bit_is_set(state.pos_status, POS_LLA_F)) {
392  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
393  /* transform lla_i -> lla_f -> utm_f, set status bits */
395  SetBit(state.pos_status, POS_LLA_F);
397  } else if (state.utm_initialized_f) {
398  if (bit_is_set(state.pos_status, POS_ENU_F)) {
400  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
402  SetBit(state.pos_status, POS_ENU_F);
404  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
406  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
408  SetBit(state.pos_status, POS_NED_F);
410  }
411  } else {
412  /* could not get this representation, set errno */
413  //struct EcefCoor_f _ecef_zero = {0.0f};
414  //return _ecef_zero;
415  return;
416  }
417  /* set bit to indicate this representation is computed */
418  SetBit(state.pos_status, POS_UTM_F);
419 }
420 
422 {
423  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
424  return;
425  }
426 
427  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
429  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
431  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
432  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
434  SetBit(state.pos_status, POS_ECEF_F);
436  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
438  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
440  SetBit(state.pos_status, POS_LLA_F);
442  } else {
443  /* could not get this representation, set errno */
444  //struct EcefCoor_f _ecef_zero = {0.0f};
445  //return _ecef_zero;
446  return;
447  }
448  /* set bit to indicate this representation is computed */
449  SetBit(state.pos_status, POS_ECEF_F);
450 }
451 
453 {
454  if (bit_is_set(state.pos_status, POS_NED_F)) {
455  return;
456  }
457 
458  int errno = 0;
459  if (state.ned_initialized_f) {
460  if (bit_is_set(state.pos_status, POS_NED_I)) {
462  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
464  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
465  /* transform ecef_i -> ned_i -> ned_f, set status bits */
467  SetBit(state.pos_status, POS_NED_I);
469  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
471  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
472  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
473  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
474  SetBit(state.pos_status, POS_ECEF_I);
476  SetBit(state.pos_status, POS_NED_I);
478  } else { /* could not get this representation, set errno */
479  errno = 1;
480  }
481  } else if (state.utm_initialized_f) {
482  if (bit_is_set(state.pos_status, POS_NED_I)) {
484  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
486  SetBit(state.pos_status, POS_ENU_F);
488  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
490  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
492  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
493  /* transform lla_f -> utm_f -> ned, set status bits */
495  SetBit(state.pos_status, POS_UTM_F);
497  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
498  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
500  SetBit(state.pos_status, POS_LLA_F);
502  SetBit(state.pos_status, POS_UTM_F);
504  } else { /* could not get this representation, set errno */
505  errno = 2;
506  }
507  } else { /* ned coordinate system not initialized, set errno */
508  errno = 3;
509  }
510  if (errno) {
511  //struct NedCoor_f _ned_zero = {0.0f};
512  //return _ned_zero;
513  return;
514  }
515  /* set bit to indicate this representation is computed */
516  SetBit(state.pos_status, POS_NED_F);
517 }
518 
520 {
521  if (bit_is_set(state.pos_status, POS_ENU_F)) {
522  return;
523  }
524 
525  int errno = 0;
526  if (state.ned_initialized_f) {
527  if (bit_is_set(state.pos_status, POS_NED_F)) {
529  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
531  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
533  SetBit(state.pos_status, POS_NED_F);
535  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
537  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
538  /* transform ecef_i -> enu_i -> enu_f, set status bits */
540  SetBit(state.pos_status, POS_ENU_I);
542  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
544  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
545  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
546  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
547  SetBit(state.pos_status, POS_ECEF_I);
549  SetBit(state.pos_status, POS_ENU_I);
551  } else { /* could not get this representation, set errno */
552  errno = 1;
553  }
554  } else if (state.utm_initialized_f) {
555  if (bit_is_set(state.pos_status, POS_ENU_I)) {
557  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
559  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
561  SetBit(state.pos_status, POS_NED_F);
563  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
565  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
566  /* transform lla_f -> utm_f -> enu, set status bits */
568  SetBit(state.pos_status, POS_UTM_F);
570  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
571  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
573  SetBit(state.pos_status, POS_LLA_F);
575  SetBit(state.pos_status, POS_UTM_F);
577  } else { /* could not get this representation, set errno */
578  errno = 2;
579  }
580  } else { /* ned coordinate system not initialized, set errno */
581  errno = 3;
582  }
583  if (errno) {
584  //struct EnuCoor_f _enu_zero = {0.0f};
585  //return _enu_zero;
586  return;
587  }
588  /* set bit to indicate this representation is computed */
589  SetBit(state.pos_status, POS_ENU_F);
590 }
591 
593 {
594  if (bit_is_set(state.pos_status, POS_LLA_F)) {
595  return;
596  }
597 
598  int errno = 0;
599  if (bit_is_set(state.pos_status, POS_LLA_I)) {
601  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
603  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
604  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
606  SetBit(state.pos_status, POS_ECEF_F);
608  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
610  } else if (state.ned_initialized_f) {
611  if (bit_is_set(state.pos_status, POS_NED_F)) {
612  /* transform ned_f -> ecef_f -> lla_f, set status bits */
614  SetBit(state.pos_status, POS_ECEF_F);
616  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
617  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
619  SetBit(state.pos_status, POS_NED_F);
621  SetBit(state.pos_status, POS_ECEF_F);
623  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
624  /* transform enu_f -> ecef_f -> lla_f, set status bits */
626  SetBit(state.pos_status, POS_ECEF_F);
628  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
629  /* transform enu_i -> enu_f -> ecef_f -> lla_f, set status bits */
631  SetBit(state.pos_status, POS_ENU_F);
633  SetBit(state.pos_status, POS_ECEF_F);
635  } else { /* could not get this representation, set errno */
636  errno = 1;
637  }
638  } else if (state.utm_initialized_f) {
639  if (bit_is_set(state.pos_status, POS_NED_I)) {
640  /* transform ned_i -> ned_f -> utm_f -> lla_f, set status bits */
642  SetBit(state.pos_status, POS_NED_F);
644  SetBit(state.pos_status, POS_UTM_F);
646  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
647  /* transform enu_i -> enu_f -> utm_f -> lla_f, set status bits */
649  SetBit(state.pos_status, POS_ENU_F);
651  SetBit(state.pos_status, POS_UTM_F);
653  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
654  /* transform ned_f -> utm_f -> lla_f, set status bits */
656  SetBit(state.pos_status, POS_UTM_F);
658  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
659  /* transform enu_f -> utm_f -> lla_f, set status bits */
661  SetBit(state.pos_status, POS_UTM_F);
663  } else { /* could not get this representation, set errno */
664  errno = 2;
665  }
666  } else { /* ned coordinate system not initialized, set errno */
667  errno = 3;
668  }
669  if (errno) {
670  /* could not get this representation, set errno */
671  //struct LlaCoor_f _lla_zero = {0.0};
672  //return _lla_zero;
673  return;
674  }
675  /* set bit to indicate this representation is computed */
676  SetBit(state.pos_status, POS_LLA_F);
677 }
684 /******************************************************************************
685  * *
686  * Transformation functions for the SPEED representations *
687  * *
688  *****************************************************************************/
691 /************************ Set functions ****************************/
692 
694 {
695  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
696  return;
697  }
698 
699  int errno = 0;
700  if (state.ned_initialized_i) {
701  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
703  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
705  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
707  SetBit(state.speed_status, SPEED_ENU_I);
709  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
711  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
712  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
716  } else { /* could not get this representation, set errno */
717  errno = 1;
718  }
719  } else if (state.utm_initialized_f) {
720  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
722  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
724  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
726  SetBit(state.speed_status, SPEED_ENU_I);
728  } else { /* could not get this representation, set errno */
729  errno = 2;
730  }
731  } else { /* ned coordinate system not initialized, set errno */
732  errno = 3;
733  }
734  if (errno) {
735  //struct NedCoor_i _ned_zero = {0};
736  //return _ned_zero;
737  return;
738  }
739  /* set bit to indicate this representation is computed */
740  SetBit(state.speed_status, SPEED_NED_I);
741 }
742 
744 {
745  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
746  return;
747  }
748 
749  int errno = 0;
750  if (state.ned_initialized_i) {
751  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
753  }
754  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
756  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
758  SetBit(state.speed_status, SPEED_NED_I);
760  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
762  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
763  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
767  } else { /* could not get this representation, set errno */
768  errno = 1;
769  }
770  } else if (state.utm_initialized_f) {
771  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
773  }
774  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
776  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
778  SetBit(state.speed_status, SPEED_NED_I);
780  } else { /* could not get this representation, set errno */
781  errno = 2;
782  }
783  } else { /* ned coordinate system not initialized, set errno */
784  errno = 3;
785  }
786  if (errno) {
787  //struct EnuCoor_i _enu_zero = {0};
788  //return _enu_zero;
789  return;
790  }
791  /* set bit to indicate this representation is computed */
792  SetBit(state.speed_status, SPEED_ENU_I);
793 }
794 
796 {
797  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
798  return;
799  }
800 
801  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
803  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
805  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
806  /* transform ned_f -> ned_i -> ecef_i , set status bits */
808  SetBit(state.speed_status, SPEED_NED_I);
810  } else {
811  /* could not get this representation, set errno */
812  //struct EcefCoor_i _ecef_zero = {0};
813  //return _ecef_zero;
814  return;
815  }
816  /* set bit to indicate this representation is computed */
818 }
819 
821 {
822  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
823  return;
824  }
825 
826  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
828  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
832  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
836  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
840  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
844  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
845  /* transform ecef speed to ned, set status bit, then compute norm */
847  SetBit(state.speed_status, SPEED_NED_I);
851  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
853  SetBit(state.speed_status, SPEED_NED_F);
857  } else {
858  //int32_t _norm_zero = 0;
859  //return _norm_zero;
860  return;
861  }
862  /* set bit to indicate this representation is computed */
864 }
865 
867 {
868  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
869  return;
870  }
871 
872  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
874  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
877  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
880  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
882  SetBit(state.speed_status, SPEED_NED_I);
885  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
887  SetBit(state.speed_status, SPEED_ENU_I);
890  } else {
891  return;
892  }
893  /* set bit to indicate this representation is computed */
895 }
896 
898 {
899  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
900  return;
901  }
902 
903  int errno = 0;
904  if (state.ned_initialized_f) {
905  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
907  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
909  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
911  SetBit(state.speed_status, SPEED_ENU_F);
913  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
915  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
916  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
920  } else { /* could not get this representation, set errno */
921  errno = 1;
922  }
923  } else if (state.utm_initialized_f) {
924  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
926  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
928  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
930  SetBit(state.speed_status, SPEED_ENU_F);
932  } else { /* could not get this representation, set errno */
933  errno = 2;
934  }
935  } else { /* ned coordinate system not initialized, set errno */
936  errno = 3;
937  }
938  if (errno) {
939  //struct NedCoor_f _ned_zero = {0.0f};
940  //return _ned_zero;
941  return;
942  }
943  /* set bit to indicate this representation is computed */
944  SetBit(state.speed_status, SPEED_NED_F);
945 }
946 
948 {
949  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
950  return;
951  }
952 
953  int errno = 0;
954  if (state.ned_initialized_f) {
955  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
957  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
959  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
961  SetBit(state.speed_status, SPEED_NED_F);
963  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
965  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
966  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
970  } else { /* could not get this representation, set errno */
971  errno = 1;
972  }
973  } else if (state.utm_initialized_f) {
974  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
976  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
978  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
980  SetBit(state.speed_status, SPEED_NED_F);
982  } else { /* could not get this representation, set errno */
983  errno = 2;
984  }
985  } else { /* ned coordinate system not initialized, set errno */
986  errno = 3;
987  }
988  if (errno) {
989  //struct EnuCoor_f _enu_zero = {0};
990  //return _enu_zero;
991  return;
992  }
993  /* set bit to indicate this representation is computed */
994  SetBit(state.speed_status, SPEED_ENU_F);
995 }
996 
998 {
999  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1000  return;
1001  }
1002 
1003  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1005  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1007  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1008  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1010  SetBit(state.speed_status, SPEED_NED_F);
1012  } else {
1013  /* could not get this representation, set errno */
1014  //struct EcefCoor_f _ecef_zero = {0.0f};
1015  //return _ecef_zero;
1016  return;
1017  }
1018  /* set bit to indicate this representation is computed */
1019  SetBit(state.speed_status, SPEED_ECEF_F);
1020 }
1021 
1023 {
1024  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
1025  return;
1026  }
1027 
1028  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
1030  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1032  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1034  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1036  SetBit(state.speed_status, SPEED_NED_F);
1038  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1040  SetBit(state.speed_status, SPEED_ENU_F);
1042  } else {
1043  return;
1044  }
1045  /* set bit to indicate this representation is computed */
1046  SetBit(state.speed_status, SPEED_HNORM_F);
1047 }
1048 
1050 {
1051  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1052  return;
1053  }
1054 
1055  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1057  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1059  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1061  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1063  SetBit(state.speed_status, SPEED_NED_F);
1065  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1067  SetBit(state.speed_status, SPEED_ENU_F);
1069  } else {
1070  return;
1071  }
1072  /* set bit to indicate this representation is computed */
1073  SetBit(state.speed_status, SPEED_HDIR_F);
1074 }
1079 /******************************************************************************
1080  * *
1081  * Transformation functions for the ACCELERATION representations *
1082  * *
1083  *****************************************************************************/
1088 {
1089  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1090  return;
1091  }
1092 
1093  int errno = 0;
1094  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1096  }
1097  else if (state.ned_initialized_i) {
1098  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1100  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1101  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1103  SetBit(state.accel_status, ACCEL_ECEF_I);
1105  } else { /* could not get this representation, set errno */
1106  errno = 1;
1107  }
1108  } else { /* ned coordinate system not initialized, set errno */
1109  errno = 2;
1110  }
1111  if (errno) {
1112  //struct NedCoor_i _ned_zero = {0};
1113  //return _ned_zero;
1114  return;
1115  }
1116  /* set bit to indicate this representation is computed */
1117  SetBit(state.accel_status, ACCEL_NED_I);
1118 }
1119 
1121 {
1122  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1123  return;
1124  }
1125 
1126  int errno = 0;
1127  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1129  }
1130  else if (state.ned_initialized_i) {
1131  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1133  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1134  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1136  SetBit(state.accel_status, ACCEL_NED_I);
1138  } else {
1139  /* could not get this representation, set errno */
1140  errno = 1;
1141  }
1142  } else { /* ned coordinate system not initialized, set errno */
1143  errno = 2;
1144  }
1145  if (errno) {
1146  return;
1147  }
1148  /* set bit to indicate this representation is computed */
1149  SetBit(state.accel_status, ACCEL_ECEF_I);
1150 }
1151 
1153 {
1154  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1155  return;
1156  }
1157 
1158  int errno = 0;
1159  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1161  }
1162  else if (state.ned_initialized_f) {
1163  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1165  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1166  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1168  SetBit(state.accel_status, ACCEL_ECEF_F);
1170  } else { /* could not get this representation, set errno */
1171  errno = 1;
1172  }
1173  } else { /* ned coordinate system not initialized, set errno */
1174  errno = 2;
1175  }
1176  if (errno) {
1177  //struct NedCoor_f _ned_zero = {0.0f};
1178  //return _ned_zero;
1179  return;
1180  }
1181  /* set bit to indicate this representation is computed */
1182  SetBit(state.accel_status, ACCEL_NED_F);
1183 }
1184 
1186 {
1187  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1188  return;
1189  }
1190 
1191  int errno = 0;
1192  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1194  }
1195  else if (state.ned_initialized_f) {
1196  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1198  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1199  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1201  SetBit(state.accel_status, ACCEL_NED_F);
1203  } else {
1204  /* could not get this representation, set errno */
1205  errno = 1;
1206  }
1207  } else { /* ned coordinate system not initialized, set errno */
1208  errno = 2;
1209  }
1210  if (errno) {
1211  return;
1212  }
1213  /* set bit to indicate this representation is computed */
1214  SetBit(state.accel_status, ACCEL_ECEF_F);
1215 }
1218 /******************************************************************************
1219  * *
1220  * Transformation functions for the ANGULAR RATE representations *
1221  * *
1222  *****************************************************************************/
1227 {
1228  if (bit_is_set(state.rate_status, RATE_I)) {
1229  return;
1230  }
1231 
1232  if (bit_is_set(state.rate_status, RATE_F)) {
1234  } else {
1235  return;
1236  }
1237  /* set bit to indicate this representation is computed */
1238  SetBit(state.rate_status, RATE_I);
1239 }
1240 
1242 {
1243  if (bit_is_set(state.rate_status, RATE_F)) {
1244  return;
1245  }
1246 
1247  if (bit_is_set(state.rate_status, RATE_I)) {
1249  } else {
1250  return;
1251  }
1252  /* set bit to indicate this representation is computed */
1253  SetBit(state.rate_status, RATE_F);
1254 }
1255 
1259 /******************************************************************************
1260  * *
1261  * Transformation functions for the WIND- AND AIRSPEED representations *
1262  * *
1263  *****************************************************************************/
1268 {
1269  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1270  return;
1271  }
1272 
1273  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1276  } else {
1277  return;
1278  }
1279  /* set bit to indicate this representation is computed */
1280  SetBit(state.wind_air_status , WINDSPEED_I);
1281 }
1282 
1284 {
1285  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1286  return;
1287  }
1288 
1289  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1291  } else {
1292  return;
1293  }
1294  /* set bit to indicate this representation is computed */
1295  SetBit(state.wind_air_status, DOWNWIND_I);
1296 }
1297 
1299 {
1300  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1301  return;
1302  }
1303 
1304  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1306  } else {
1307  return;
1308  }
1309  /* set bit to indicate this representation is computed */
1310  SetBit(state.wind_air_status, AIRSPEED_I);
1311 }
1312 
1314 {
1315  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1316  return;
1317  }
1318 
1319  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1322  } else {
1323  return;
1324  }
1325  /* set bit to indicate this representation is computed */
1327 }
1328 
1330 {
1331  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1332  return;
1333  }
1334 
1335  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1337  } else {
1338  return;
1339  }
1340  /* set bit to indicate this representation is computed */
1341  SetBit(state.wind_air_status, DOWNWIND_F);
1342 }
1343 
1345 {
1346  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1347  return;
1348  }
1349 
1350  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1352  } else {
1353  return;
1354  }
1355  /* set bit to indicate this representation is computed */
1356  SetBit(state.wind_air_status, AIRSPEED_F);
1357 }
#define FLOAT_VECT2_NORM(_v)
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:789
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:783
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
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)
Definition: pprz_geodetic.h:96
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:78
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:84
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:90
int32_t y
North.
int32_t y
East.
int32_t x
East.
int32_t x
North.
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 LLA_FLOAT_OF_BFP(_o, _i)
#define INT32_VECT3_NED_OF_ENU(_o, _i)
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.
uint8_t status
Holds the status bits for all orientation representations.
#define ACCEL_ECEF_I
Definition: state.h:102
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
#define ACCEL_NED_F
Definition: state.h:105
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
void stateCalcAccelNed_i(void)
Definition: state.c:1087
void stateCalcAccelEcef_i(void)
Definition: state.c:1120
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
#define ACCEL_ECEF_F
Definition: state.h:104
void stateCalcAccelEcef_f(void)
Definition: state.c:1185
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
#define ACCEL_NED_I
Definition: state.h:103
void stateCalcAccelNed_f(void)
Definition: state.c:1152
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
struct State state
Definition: state.c:36
void stateInit(void)
Definition: state.c:57
Structure holding vehicle state data.
Definition: state.h:134
#define POS_LLA_F
Definition: state.h:75
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
#define POS_UTM_F
Definition: state.h:76
#define POS_LLA_I
Definition: state.h:70
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:243
#define POS_ENU_F
Definition: state.h:74
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
#define POS_ECEF_F
Definition: state.h:72
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
void stateCalcPositionNed_f(void)
Definition: state.c:452
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:144
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
void stateCalcPositionUtm_f(void)
Definition: state.c:384
void stateCalcPositionEnu_f(void)
Definition: state.c:519
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
#define POS_ENU_I
Definition: state.h:69
#define POS_ECEF_I
Definition: state.h:67
void stateCalcPositionEcef_f(void)
Definition: state.c:421
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
#define POS_NED_F
Definition: state.h:73
void stateCalcPositionLla_f(void)
Definition: state.c:592
void stateCalcPositionEcef_i(void)
Definition: state.c:87
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
void stateCalcPositionNed_i(void)
Definition: state.c:119
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:286
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:150
#define POS_NED_I
Definition: state.h:68
void stateCalcPositionEnu_i(void)
Definition: state.c:199
void stateCalcBodyRates_f(void)
Definition: state.c:1241
#define RATE_F
Definition: state.h:113
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
#define RATE_I
Definition: state.h:112
void stateCalcBodyRates_i(void)
Definition: state.c:1226
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
#define SPEED_ENU_F
Definition: state.h:92
#define SPEED_ENU_I
Definition: state.h:87
void stateCalcSpeedEnu_f(void)
Definition: state.c:947
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
#define SPEED_ECEF_F
Definition: state.h:90
void stateCalcSpeedNed_f(void)
Definition: state.c:897
#define SPEED_ECEF_I
Definition: state.h:85
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:866
#define SPEED_NED_F
Definition: state.h:91
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:1022
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:820
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
void stateCalcSpeedEcef_f(void)
Definition: state.c:997
#define SPEED_HDIR_I
Definition: state.h:89
void stateCalcSpeedEnu_i(void)
Definition: state.c:743
#define SPEED_HNORM_F
Definition: state.h:93
#define SPEED_NED_I
Definition: state.h:86
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1049
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
void stateCalcSpeedNed_i(void)
Definition: state.c:693
#define SPEED_HNORM_I
Definition: state.h:88
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
#define SPEED_HDIR_F
Definition: state.h:94
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
void stateCalcSpeedEcef_i(void)
Definition: state.c:795
#define WINDSPEED_I
Definition: state.h:120
int32_t airspeed_i
Norm of relative wind speed.
Definition: state.h:416
#define DOWNWIND_F
Definition: state.h:124
#define WINDSPEED_F
Definition: state.h:123
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
union State::@344 windspeed_f
Horizontal windspeed.
#define AIRSPEED_I
Definition: state.h:122
union State::@343 windspeed_i
Horizontal windspeed in north/east/down.
void stateCalcAirspeed_i(void)
Definition: state.c:1298
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1283
void stateCalcAirspeed_f(void)
Definition: state.c:1344
#define AIRSPEED_F
Definition: state.h:125
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1313
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1267
#define DOWNWIND_I
Definition: state.h:121
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1329
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)
float y
in meters
float x
in meters
uint8_t zone
UTM zone number.
float x
in meters
float y
in meters
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 int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78