Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
43 void stateInit(void) {
44  state.pos_status = 0;
45  state.speed_status = 0;
46  state.accel_status = 0;
48  state.rate_status = 0;
53 }
54 
55 
56 /*******************************************************************************
57  * *
58  * transformation functions for the POSITION representations *
59  * *
60  ******************************************************************************/
65  if (bit_is_set(state.pos_status, POS_ECEF_I))
66  return;
67 
68  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
70  }
71  else if (bit_is_set(state.pos_status, POS_NED_I)) {
73  }
74  else if (bit_is_set(state.pos_status, POS_NED_F)) {
75  /* transform ned_f to ecef_f, set status bit, then convert to int */
77  SetBit(state.pos_status, POS_ECEF_F);
79  }
80  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
81  /* transform lla_f to ecef_f, set status bit, then convert to int */
83  SetBit(state.pos_status, POS_ECEF_F);
85  }
86  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
88  }
89  else {
90  /* could not get this representation, set errno */
91  //struct EcefCoor_i _ecef_zero = {0};
92  //return _ecef_zero;
93  }
94  /* set bit to indicate this representation is computed */
95  SetBit(state.pos_status, POS_ECEF_I);
96 }
97 
99  if (bit_is_set(state.pos_status, POS_NED_I))
100  return;
101 
102  int errno = 0;
103  if (state.ned_initialized_i) {
104  if (bit_is_set(state.pos_status, POS_NED_F)) {
106  }
107  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
109  }
110  else if (bit_is_set(state.pos_status, POS_ENU_F)) {
112  SetBit(state.pos_status, POS_ENU_I);
114  }
115  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
117  }
118  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
119  /* transform ecef_f -> ned_f, set status bit, then convert to int */
121  SetBit(state.pos_status, POS_NED_F);
123  }
124  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
125  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
127  SetBit(state.pos_status, POS_ECEF_F);
129  SetBit(state.pos_status, POS_NED_F);
131  }
132  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
134  }
135  else { /* could not get this representation, set errno */
136  errno = 1;
137  }
138  }
139  else if (state.utm_initialized_f) {
140  if (bit_is_set(state.pos_status, POS_NED_F)) {
142  }
143  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
145  }
146  else if (bit_is_set(state.pos_status, POS_ENU_F)) {
148  SetBit(state.pos_status, POS_ENU_I);
150  }
151  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
152  /* transform utm_f -> ned_f -> ned_i, set status bits */
154  SetBit(state.pos_status, POS_NED_F);
156  }
157  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
158  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
160  SetBit(state.pos_status, POS_UTM_F);
162  SetBit(state.pos_status, POS_NED_F);
164  }
165  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
166  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
168  SetBit(state.pos_status, POS_LLA_F);
170  SetBit(state.pos_status, POS_UTM_F);
172  SetBit(state.pos_status, POS_NED_F);
174  }
175  else { /* could not get this representation, set errno */
176  errno = 2;
177  }
178  }
179  else { /* ned coordinate system not initialized, set errno */
180  errno = 3;
181  }
182  if (errno) {
183  //struct NedCoor_i _ned_zero = {0};
184  //return _ned_zero;
185  }
186  /* set bit to indicate this representation is computed */
187  SetBit(state.pos_status, POS_NED_I);
188 }
189 
191  if (bit_is_set(state.pos_status, POS_ENU_I))
192  return;
193 
194  int errno = 0;
195  if (state.ned_initialized_i) {
196  if (bit_is_set(state.pos_status, POS_NED_I)) {
198  }
199  else if (bit_is_set(state.pos_status, POS_ENU_F)) {
201  }
202  else if (bit_is_set(state.pos_status, POS_NED_F)) {
204  SetBit(state.pos_status, POS_NED_I);
206  }
207  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
209  }
210  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
211  /* transform ecef_f -> enu_f, set status bit, then convert to int */
213  SetBit(state.pos_status, POS_ENU_F);
215  }
216  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
217  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
219  SetBit(state.pos_status, POS_ECEF_F);
221  SetBit(state.pos_status, POS_ENU_F);
223  }
224  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
226  }
227  else { /* could not get this representation, set errno */
228  errno = 1;
229  }
230  }
231  else if (state.utm_initialized_f) {
232  if (bit_is_set(state.pos_status, POS_ENU_F)) {
234  }
235  else if (bit_is_set(state.pos_status, POS_NED_I)) {
237  }
238  else if (bit_is_set(state.pos_status, POS_NED_F)) {
240  SetBit(state.pos_status, POS_NED_I);
242  }
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  }
249  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
250  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
252  SetBit(state.pos_status, POS_UTM_F);
254  SetBit(state.pos_status, POS_ENU_F);
256  }
257  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
258  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
260  SetBit(state.pos_status, POS_LLA_F);
262  SetBit(state.pos_status, POS_UTM_F);
264  SetBit(state.pos_status, POS_ENU_F);
266  }
267  else { /* could not get this representation, set errno */
268  errno = 2;
269  }
270  }
271  else { /* ned coordinate system not initialized, set errno */
272  errno = 3;
273  }
274  if (errno) {
275  //struct EnuCoor_i _enu_zero = {0};
276  //return _enu_zero;
277  }
278  /* set bit to indicate this representation is computed */
279  SetBit(state.pos_status, POS_ENU_I);
280 }
281 
283  if (bit_is_set(state.pos_status, POS_LLA_I))
284  return;
285 
286  if (bit_is_set(state.pos_status, POS_LLA_F)) {
288  }
289  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
291  }
292  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
293  /* transform ecef_f -> lla_f, set status bit, then convert to int */
295  SetBit(state.pos_status, POS_LLA_F);
297  }
298  else if (bit_is_set(state.pos_status, POS_NED_F)) {
299  /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
301  SetBit(state.pos_status, POS_ECEF_F);
303  SetBit(state.pos_status, POS_LLA_F);
305  }
306  else 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);
310  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
311  }
312  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
313  /* transform utm_f -> lla_f -> lla_i, set status bits */
315  SetBit(state.pos_status, POS_LLA_F);
317  }
318  else {
319  /* could not get this representation, set errno */
320  //struct LlaCoor_i _lla_zero = {0};
321  //return _lla_zero;
322  }
323  /* set bit to indicate this representation is computed */
324  SetBit(state.pos_status, POS_LLA_I);
325 }
326 
328  if (bit_is_set(state.pos_status, POS_UTM_F))
329  return;
330 
331  if (bit_is_set(state.pos_status, POS_LLA_F)) {
333  }
334  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
335  /* transform lla_i -> lla_f -> utm_f, set status bits */
337  SetBit(state.pos_status, POS_LLA_F);
339  }
340  else if (state.utm_initialized_f) {
341  if (bit_is_set(state.pos_status, POS_ENU_F)) {
343  }
344  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
346  SetBit(state.pos_status, POS_ENU_F);
348  }
349  else if (bit_is_set(state.pos_status, POS_NED_F)) {
351  }
352  else if (bit_is_set(state.pos_status, POS_NED_I)) {
354  SetBit(state.pos_status, POS_NED_F);
356  }
357  }
358  else {
359  /* could not get this representation, set errno */
360  //struct EcefCoor_f _ecef_zero = {0.0f};
361  //return _ecef_zero;
362  }
363  /* set bit to indicate this representation is computed */
364  SetBit(state.pos_status, POS_UTM_F);
365 }
366 
368  if (bit_is_set(state.pos_status, POS_ECEF_F))
369  return;
370 
371  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
373  }
374  else if (bit_is_set(state.pos_status, POS_NED_F)) {
376  }
377  else if (bit_is_set(state.pos_status, POS_NED_I)) {
378  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
380  SetBit(state.pos_status, POS_ECEF_F);
382  }
383  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
385  }
386  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
388  SetBit(state.pos_status, POS_LLA_F);
390  }
391  else {
392  /* could not get this representation, set errno */
393  //struct EcefCoor_f _ecef_zero = {0.0f};
394  //return _ecef_zero;
395  }
396  /* set bit to indicate this representation is computed */
397  SetBit(state.pos_status, POS_ECEF_F);
398 }
399 
401  if (bit_is_set(state.pos_status, POS_NED_F))
402  return;
403 
404  int errno = 0;
405  if (state.ned_initialized_f) {
406  if (bit_is_set(state.pos_status, POS_NED_I)) {
408  }
409  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
411  }
412  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
413  /* transform ecef_i -> ned_i -> ned_f, set status bits */
415  SetBit(state.pos_status, POS_NED_I);
417  }
418  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
420  }
421  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
422  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
423  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
424  SetBit(state.pos_status, POS_ECEF_I);
426  SetBit(state.pos_status, POS_NED_I);
428  }
429  else { /* could not get this representation, set errno */
430  errno = 1;
431  }
432  }
433  else if (state.utm_initialized_f) {
434  if (bit_is_set(state.pos_status, POS_NED_I)) {
436  }
437  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
439  SetBit(state.pos_status, POS_ENU_F);
441  }
442  else if (bit_is_set(state.pos_status, POS_ENU_F)) {
444  }
445  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
447  }
448  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
449  /* transform lla_f -> utm_f -> ned, set status bits */
451  SetBit(state.pos_status, POS_UTM_F);
453  }
454  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
455  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
457  SetBit(state.pos_status, POS_LLA_F);
459  SetBit(state.pos_status, POS_UTM_F);
461  }
462  else { /* could not get this representation, set errno */
463  errno = 2;
464  }
465  }
466  else { /* ned coordinate system not initialized, set errno */
467  errno = 3;
468  }
469  if (errno) {
470  //struct NedCoor_f _ned_zero = {0.0f};
471  //return _ned_zero;
472  }
473  /* set bit to indicate this representation is computed */
474  SetBit(state.pos_status, POS_NED_F);
475 }
476 
478  if (bit_is_set(state.pos_status, POS_ENU_F))
479  return;
480 
481  int errno = 0;
482  if (state.ned_initialized_f) {
483  if (bit_is_set(state.pos_status, POS_NED_F)) {
485  }
486  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
488  }
489  else if (bit_is_set(state.pos_status, POS_NED_I)) {
491  SetBit(state.pos_status, POS_NED_F);
493  }
494  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
496  }
497  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
498  /* transform ecef_i -> enu_i -> enu_f, set status bits */
500  SetBit(state.pos_status, POS_ENU_I);
502  }
503  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
505  }
506  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
507  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
508  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
509  SetBit(state.pos_status, POS_ECEF_I);
511  SetBit(state.pos_status, POS_ENU_I);
513  }
514  else { /* could not get this representation, set errno */
515  errno = 1;
516  }
517  }
518  else if (state.utm_initialized_f) {
519  if (bit_is_set(state.pos_status, POS_ENU_I)) {
521  }
522  else if (bit_is_set(state.pos_status, POS_NED_F)) {
524  }
525  else if (bit_is_set(state.pos_status, POS_NED_I)) {
527  SetBit(state.pos_status, POS_NED_F);
529  }
530  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
532  }
533  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
534  /* transform lla_f -> utm_f -> enu, set status bits */
536  SetBit(state.pos_status, POS_UTM_F);
538  }
539  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
540  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
542  SetBit(state.pos_status, POS_LLA_F);
544  SetBit(state.pos_status, POS_UTM_F);
546  }
547  else { /* could not get this representation, set errno */
548  errno = 2;
549  }
550  }
551  else { /* ned coordinate system not initialized, set errno */
552  errno = 3;
553  }
554  if (errno) {
555  //struct EnuCoor_f _enu_zero = {0.0f};
556  //return _enu_zero;
557  }
558  /* set bit to indicate this representation is computed */
559  SetBit(state.pos_status, POS_ENU_F);
560 }
561 
563  if (bit_is_set(state.pos_status, POS_LLA_F))
564  return;
565 
566  if (bit_is_set(state.pos_status, POS_LLA_I)) {
568  }
569  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
571  }
572  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
573  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
575  SetBit(state.pos_status, POS_ECEF_F);
577  }
578  else if (bit_is_set(state.pos_status, POS_NED_F)) {
579  /* transform ned_f -> ecef_f -> lla_f, set status bits */
581  SetBit(state.pos_status, POS_ECEF_F);
583  }
584  else if (bit_is_set(state.pos_status, POS_NED_I)) {
585  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
587  SetBit(state.pos_status, POS_NED_F);
589  SetBit(state.pos_status, POS_ECEF_F);
591  }
592  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
594  }
595  else {
596  /* could not get this representation, set errno */
597  //struct LlaCoor_f _lla_zero = {0.0};
598  //return _lla_zero;
599  }
600  /* set bit to indicate this representation is computed */
601  SetBit(state.pos_status, POS_LLA_F);
602 }
609 /******************************************************************************
610  * *
611  * Transformation functions for the SPEED representations *
612  * *
613  *****************************************************************************/
616 /************************ Set functions ****************************/
617 
619  if (bit_is_set(state.speed_status, SPEED_NED_I))
620  return;
621 
622  int errno = 0;
623  if (state.ned_initialized_i) {
624  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
626  }
627  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
629  }
630  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
632  SetBit(state.speed_status, SPEED_ENU_I);
634  }
635  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
637  }
638  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
639  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
643  }
644  else { /* could not get this representation, set errno */
645  errno = 1;
646  }
647  }
648  else if (state.utm_initialized_f) {
649  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
651  }
652  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
654  }
655  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
657  SetBit(state.speed_status, SPEED_ENU_I);
659  }
660  else { /* could not get this representation, set errno */
661  errno = 2;
662  }
663  }
664  else { /* ned coordinate system not initialized, set errno */
665  errno = 3;
666  }
667  if (errno) {
668  //struct NedCoor_i _ned_zero = {0};
669  //return _ned_zero;
670  }
671  /* set bit to indicate this representation is computed */
672  SetBit(state.speed_status, SPEED_NED_I);
673 }
674 
676  if (bit_is_set(state.speed_status, SPEED_ENU_I))
677  return;
678 
679  int errno = 0;
680  if (state.ned_initialized_i) {
681  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
683  }
684  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
686  }
687  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
689  SetBit(state.pos_status, SPEED_NED_I);
691  }
692  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
694  }
695  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
696  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
700  }
701  else { /* could not get this representation, set errno */
702  errno = 1;
703  }
704  }
705  else if (state.utm_initialized_f) {
706  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
708  }
709  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
711  }
712  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
714  SetBit(state.pos_status, SPEED_NED_I);
716  }
717  else { /* could not get this representation, set errno */
718  errno = 2;
719  }
720  }
721  else { /* ned coordinate system not initialized, set errno */
722  errno = 3;
723  }
724  if (errno) {
725  //struct EnuCoor_i _enu_zero = {0};
726  //return _enu_zero;
727  }
728  /* set bit to indicate this representation is computed */
729  SetBit(state.speed_status, SPEED_ENU_I);
730 }
731 
733  if (bit_is_set(state.speed_status, SPEED_ECEF_I))
734  return;
735 
736  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
738  }
739  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
741  }
742  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
743  /* transform ned_f -> ned_i -> ecef_i , set status bits */
745  SetBit(state.speed_status, SPEED_NED_I);
747  }
748  else {
749  /* could not get this representation, set errno */
750  //struct EcefCoor_i _ecef_zero = {0};
751  //return _ecef_zero;
752  }
753  /* set bit to indicate this representation is computed */
755 }
756 
758  if (bit_is_set(state.speed_status, SPEED_HNORM_I))
759  return;
760 
761  if (bit_is_set(state.speed_status, SPEED_HNORM_F)){
763  }
764  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
768  }
769  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
773  }
774  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
778  }
779  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
783  }
784  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
785  /* transform ecef speed to ned, set status bit, then compute norm */
787  SetBit(state.speed_status, SPEED_NED_I);
791  }
792  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
794  SetBit(state.speed_status, SPEED_NED_F);
798  }
799  else {
800  //int32_t _norm_zero = 0;
801  //return _norm_zero;
802  }
803  /* set bit to indicate this representation is computed */
805 }
806 
808  if (bit_is_set(state.speed_status, SPEED_HDIR_I))
809  return;
810 
811  if (bit_is_set(state.speed_status, SPEED_HDIR_F)){
813  }
814  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
817  }
818  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
821  }
822  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
824  SetBit(state.speed_status, SPEED_NED_I);
827  }
828  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
830  SetBit(state.speed_status, SPEED_ENU_I);
833  }
834  /* set bit to indicate this representation is computed */
836 }
837 
839  if (bit_is_set(state.speed_status, SPEED_NED_F))
840  return;
841 
842  int errno = 0;
843  if (state.ned_initialized_f) {
844  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
846  }
847  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
849  }
850  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
852  SetBit(state.speed_status, SPEED_ENU_F);
854  }
855  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
857  }
858  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
859  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
863  }
864  else { /* could not get this representation, set errno */
865  errno = 1;
866  }
867  }
868  else if (state.utm_initialized_f) {
869  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
871  }
872  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
874  }
875  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
877  SetBit(state.speed_status, SPEED_ENU_F);
879  }
880  else { /* could not get this representation, set errno */
881  errno = 2;
882  }
883  }
884  else { /* ned coordinate system not initialized, set errno */
885  errno = 3;
886  }
887  if (errno) {
888  //struct NedCoor_f _ned_zero = {0.0f};
889  //return _ned_zero;
890  }
891  /* set bit to indicate this representation is computed */
892  SetBit(state.speed_status, SPEED_NED_F);
893 }
894 
896  if (bit_is_set(state.speed_status, SPEED_ENU_F))
897  return;
898 
899  int errno = 0;
900  if (state.ned_initialized_f) {
901  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
903  }
904  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
906  }
907  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
909  SetBit(state.pos_status, SPEED_NED_F);
911  }
912  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
914  }
915  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
916  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
920  }
921  else { /* could not get this representation, set errno */
922  errno = 1;
923  }
924  }
925  else if (state.utm_initialized_f) {
926  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
928  }
929  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
931  }
932  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
934  SetBit(state.pos_status, SPEED_NED_F);
936  }
937  else { /* could not get this representation, set errno */
938  errno = 2;
939  }
940  }
941  else { /* ned coordinate system not initialized, set errno */
942  errno = 3;
943  }
944  if (errno) {
945  //struct EnuCoor_f _enu_zero = {0};
946  //return _enu_zero;
947  }
948  /* set bit to indicate this representation is computed */
949  SetBit(state.speed_status, SPEED_ENU_F);
950 }
951 
953  if (bit_is_set(state.speed_status, SPEED_ECEF_F))
954  return;
955 
956  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
958  }
959  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
961  }
962  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
963  /* transform ned_f -> ned_i -> ecef_i , set status bits */
965  SetBit(state.speed_status, SPEED_NED_F);
967  }
968  else {
969  /* could not get this representation, set errno */
970  //struct EcefCoor_f _ecef_zero = {0.0f};
971  //return _ecef_zero;
972  }
973  /* set bit to indicate this representation is computed */
975 }
976 
978  if (bit_is_set(state.speed_status, SPEED_HNORM_F))
979  return;
980 
981  if (bit_is_set(state.speed_status, SPEED_HNORM_I)){
983  }
984  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
986  }
987  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
989  }
990  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
992  SetBit(state.speed_status, SPEED_NED_F);
994  }
995  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
997  SetBit(state.speed_status, SPEED_ENU_F);
999  }
1000  /* set bit to indicate this representation is computed */
1001  SetBit(state.speed_status, SPEED_HNORM_F);
1002 }
1003 
1005  if (bit_is_set(state.speed_status, SPEED_HDIR_F))
1006  return;
1007 
1008  if (bit_is_set(state.speed_status, SPEED_HDIR_I)){
1010  }
1011  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1013  }
1014  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1016  }
1017  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1019  SetBit(state.speed_status, SPEED_NED_F);
1021  }
1022  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1024  SetBit(state.speed_status, SPEED_ENU_F);
1026  }
1027  /* set bit to indicate this representation is computed */
1028  SetBit(state.speed_status, SPEED_HDIR_F);
1029 }
1034 /******************************************************************************
1035  * *
1036  * Transformation functions for the ACCELERATION representations *
1037  * *
1038  *****************************************************************************/
1043  if (bit_is_set(state.accel_status, ACCEL_NED_I))
1044  return;
1045 
1046  int errno = 0;
1047  if (state.ned_initialized_i) {
1048  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1050  }
1051  else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1053  }
1054  else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1055  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1057  SetBit(state.accel_status, ACCEL_ECEF_I);
1059  }
1060  else { /* could not get this representation, set errno */
1061  errno = 1;
1062  }
1063  } else { /* ned coordinate system not initialized, set errno */
1064  errno = 2;
1065  }
1066  if (errno) {
1067  //struct NedCoor_i _ned_zero = {0};
1068  //return _ned_zero;
1069  }
1070  /* set bit to indicate this representation is computed */
1071  SetBit(state.accel_status, ACCEL_NED_I);
1072 }
1073 
1075  if (bit_is_set(state.accel_status, ACCEL_ECEF_I))
1076  return;
1077 
1078  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1080  }
1081  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1083  }
1084  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1085  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1087  SetBit(state.accel_status, ACCEL_NED_I);
1089  }
1090  else {
1091  /* could not get this representation, set errno */
1092  //struct EcefCoor_i _ecef_zero = {0};
1093  //return _ecef_zero;
1094  }
1095  /* set bit to indicate this representation is computed */
1096  SetBit(state.accel_status, ACCEL_ECEF_I);
1097 }
1098 
1100  if (bit_is_set(state.accel_status, ACCEL_NED_F))
1101  return;
1102 
1103  int errno = 0;
1104  if (state.ned_initialized_f) {
1105  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1107  }
1108  else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1110  }
1111  else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1112  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1114  SetBit(state.accel_status, ACCEL_ECEF_F);
1116  }
1117  else { /* could not get this representation, set errno */
1118  errno = 1;
1119  }
1120  } else { /* ned coordinate system not initialized, set errno */
1121  errno = 2;
1122  }
1123  if (errno) {
1124  //struct NedCoor_f _ned_zero = {0.0f};
1125  //return _ned_zero;
1126  }
1127  /* set bit to indicate this representation is computed */
1128  SetBit(state.accel_status, ACCEL_NED_F);
1129 }
1130 
1132  if (bit_is_set(state.accel_status, ACCEL_ECEF_F))
1133  return;
1134 
1135  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1137  }
1138  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1140  }
1141  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1142  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1144  SetBit(state.accel_status, ACCEL_NED_F);
1146  }
1147  else {
1148  /* could not get this representation, set errno */
1149  //struct EcefCoor_f _ecef_zero = {0.0f};
1150  //return _ecef_zero;
1151  }
1152  /* set bit to indicate this representation is computed */
1153  SetBit(state.accel_status, ACCEL_ECEF_F);
1154 }
1157 /******************************************************************************
1158  * *
1159  * Transformation functions for the ANGULAR RATE representations *
1160  * *
1161  *****************************************************************************/
1166  if (bit_is_set(state.rate_status, RATE_I))
1167  return;
1168 
1169  if (bit_is_set(state.rate_status, RATE_F)) {
1171  }
1172  /* set bit to indicate this representation is computed */
1173  SetBit(state.rate_status, RATE_I);
1174 }
1175 
1177  if (bit_is_set(state.rate_status, RATE_F))
1178  return;
1179 
1180  if (bit_is_set(state.rate_status, RATE_I)) {
1182  }
1183  /* set bit to indicate this representation is computed */
1184  SetBit(state.rate_status, RATE_F);
1185 }
1186 
1190 /******************************************************************************
1191  * *
1192  * Transformation functions for the WIND- AND AIRSPEED representations *
1193  * *
1194  *****************************************************************************/
1199  if (bit_is_set(state.wind_air_status, WINDSPEED_I))
1200  return;
1201 
1202  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1205  }
1206  /* set bit to indicate this representation is computed */
1207  SetBit(state.rate_status, WINDSPEED_I);
1208 }
1209 
1211  if (bit_is_set(state.wind_air_status, AIRSPEED_I))
1212  return;
1213 
1214  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1216  }
1217  /* set bit to indicate this representation is computed */
1218  SetBit(state.wind_air_status, AIRSPEED_I);
1219 }
1220 
1222  if (bit_is_set(state.wind_air_status, WINDSPEED_F))
1223  return;
1224 
1225  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1228  }
1229  /* set bit to indicate this representation is computed */
1230  SetBit(state.rate_status, WINDSPEED_F);
1231 }
1232 
1234  if (bit_is_set(state.wind_air_status, AIRSPEED_F))
1235  return;
1236 
1237  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1239  }
1240  /* set bit to indicate this representation is computed */
1241  SetBit(state.wind_air_status, AIRSPEED_F);
1242 }
#define POS_ENU_F
Definition: state.h:72
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:274
#define ACCEL_ECEF_I
Definition: state.h:100
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 POS_LLA_I
Definition: state.h:68
#define RATE_F
Definition: state.h:111
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
void stateCalcAccelNed_i(void)
Definition: state.c:1042
#define AIRSPEED_F
Definition: state.h:121
#define WINDSPEED_I
Definition: state.h:118
struct FloatVect2 h_windspeed_f
Horizontal windspeed.
Definition: state.h:408
#define VECT3_ENU_OF_NED(_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.
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:316
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1198
float airspeed_f
Norm of relative air speed.
Definition: state.h:414
void stateCalcAirspeed_i(void)
Definition: state.c:1210
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
#define ECEF_BFP_OF_REAL(_o, _i)
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:146
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:379
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
#define SPEED_ENU_F
Definition: state.h:90
float y
in meters
void stateCalcSpeedEnu_f(void)
Definition: state.c:895
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:153
#define POS_UTM_F
Definition: state.h:74
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:201
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:207
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:310
#define SPEED_NED_I
Definition: state.h:84
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:339
bool_t ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:219
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:268
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:367
int32_t x
North.
#define POS_NED_F
Definition: state.h:71
void stateCalcPositionEcef_f(void)
Definition: state.c:367
#define INT32_SQRT(_out, _in)
struct Int32Vect2 h_windspeed_i
Horizontal windspeed in north/east.
Definition: state.h:396
void stateCalcBodyRates_i(void)
Definition: state.c:1165
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:287
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 ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a ECEF position to local NED.
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
#define POS_NED_I
Definition: state.h:66
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:757
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
#define INT32_VECT3_NED_OF_ENU(_o, _i)
float y
in meters
uint8_t status
Holds the status bits for all orientation representations.
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.
void stateCalcSpeedNed_f(void)
Definition: state.c:838
#define NED_FLOAT_OF_BFP(_o, _i)
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
#define ENU_BFP_OF_REAL(_o, _i)
void stateInit(void)
Definition: state.c:43
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:238
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:333
float x
in meters
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:702
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
void stateCalcSpeedEnu_i(void)
Definition: state.c:675
#define ACCEL_ECEF_F
Definition: state.h:102
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:256
#define SPEED_FLOAT_OF_BFP(_ai)
#define ECEF_FLOAT_OF_BFP(_o, _i)
#define SPEED_HNORM_I
Definition: state.h:86
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:678
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:977
#define SPEED_HDIR_I
Definition: state.h:87
#define AIRSPEED_I
Definition: state.h:119
void stateCalcPositionEnu_i(void)
Definition: state.c:190
#define SPEED_ECEF_I
Definition: state.h:83
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void stateCalcPositionLla_i(void)
Definition: state.c:282
#define INT32_ATAN2(_a, _y, _x)
float x
in meters
#define LLA_BFP_OF_REAL(_o, _i)
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:327
void stateCalcSpeedEcef_i(void)
Definition: state.c:732
void stateCalcPositionUtm_f(void)
Definition: state.c:327
void stateCalcPositionNed_f(void)
Definition: state.c:400
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:50
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:807
int32_t y
East.
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:304
int32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:280
void stateCalcAccelEcef_f(void)
Definition: state.c:1131
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:672
void stateCalcAccelNed_f(void)
Definition: state.c:1099
struct OrientationReps ned_to_body_orientation
Definition: state.h:357
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:696
void stateCalcBodyRates_f(void)
Definition: state.c:1176
signed long int32_t
Definition: types.h:19
#define ACCEL_NED_F
Definition: state.h:103
#define POS_ECEF_I
Definition: state.h:65
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
void stateCalcAccelEcef_i(void)
Definition: state.c:1074
#define NED_BFP_OF_REAL(_o, _i)
#define INT32_COURSE_NORMALIZE(_a)
int32_t airspeed_i
Norm of horizontal ground speed.
Definition: state.h:402
#define SPEED_ENU_I
Definition: state.h:85
#define WINDSPEED_F
Definition: state.h:120
void stateCalcSpeedEcef_f(void)
Definition: state.c:952
#define POS_LLA_F
Definition: state.h:73
bool_t utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:231
API to get/set the generic vehicle states.
#define FLOAT_VECT2_NORM(_n, _v)
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:373
void stateCalcPositionEnu_f(void)
Definition: state.c:477
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:390
#define LLA_FLOAT_OF_BFP(_o, _i)
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1004
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:345
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
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.
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
#define SPEED_NED_F
Definition: state.h:89
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:188
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:56
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:293
#define ENU_FLOAT_OF_BFP(_o, _i)
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:38
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:262
void stateCalcPositionNed_i(void)
Definition: state.c:98
Structure holding vehicle state data.
Definition: state.h:130
void stateCalcAirspeed_f(void)
Definition: state.c:1233
void stateCalcSpeedNed_i(void)
Definition: state.c:618
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:298
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:181
#define INT32_SPEED_FRAC
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:140
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:44
#define SPEED_HDIR_F
Definition: state.h:92
int32_t y
North.
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
void stateCalcPositionEcef_i(void)
Definition: state.c:64
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:245
#define VECT3_NED_OF_ENU(_o, _i)
void stateCalcPositionLla_f(void)
Definition: state.c:562
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:351
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
#define POS_ECEF_F
Definition: state.h:70
#define SPEED_BFP_OF_REAL(_af)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:714
#define SPEED_ECEF_F
Definition: state.h:88
#define RATE_I
Definition: state.h:110
#define ACCEL_NED_I
Definition: state.h:101
#define POS_ENU_I
Definition: state.h:67
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:174
int32_t x
East.
#define SPEED_HNORM_F
Definition: state.h:91
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define INT32_VECT3_ENU_OF_NED(_o, _i)
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1221