Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 {
341  /* could not get this representation, set errno */
342  //struct EcefCoor_f _ecef_zero = {0.0f};
343  //return _ecef_zero;
344  }
345  /* set bit to indicate this representation is computed */
346  SetBit(state.pos_status, POS_UTM_F);
347 }
348 
350  if (bit_is_set(state.pos_status, POS_ECEF_F))
351  return;
352 
353  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
355  }
356  else if (bit_is_set(state.pos_status, POS_NED_F)) {
358  }
359  else if (bit_is_set(state.pos_status, POS_NED_I)) {
360  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
362  SetBit(state.pos_status, POS_ECEF_F);
364  }
365  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
367  }
368  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
370  SetBit(state.pos_status, POS_LLA_F);
372  }
373  else {
374  /* could not get this representation, set errno */
375  //struct EcefCoor_f _ecef_zero = {0.0f};
376  //return _ecef_zero;
377  }
378  /* set bit to indicate this representation is computed */
379  SetBit(state.pos_status, POS_ECEF_F);
380 }
381 
383  if (bit_is_set(state.pos_status, POS_NED_F))
384  return;
385 
386  int errno = 0;
387  if (state.ned_initialized_f) {
388  if (bit_is_set(state.pos_status, POS_NED_I)) {
390  }
391  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
393  }
394  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
395  /* transform ecef_i -> ned_i -> ned_f, set status bits */
397  SetBit(state.pos_status, POS_NED_I);
399  }
400  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
402  }
403  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
404  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
405  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
406  SetBit(state.pos_status, POS_ECEF_I);
408  SetBit(state.pos_status, POS_NED_I);
410  }
411  else { /* could not get this representation, set errno */
412  errno = 1;
413  }
414  }
415  else if (state.utm_initialized_f) {
416  if (bit_is_set(state.pos_status, POS_NED_I)) {
418  }
419  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
421  SetBit(state.pos_status, POS_ENU_F);
423  }
424  else if (bit_is_set(state.pos_status, POS_ENU_F)) {
426  }
427  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
429  }
430  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
431  /* transform lla_f -> utm_f -> ned, set status bits */
433  SetBit(state.pos_status, POS_UTM_F);
435  }
436  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
437  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
439  SetBit(state.pos_status, POS_LLA_F);
441  SetBit(state.pos_status, POS_UTM_F);
443  }
444  else { /* could not get this representation, set errno */
445  errno = 2;
446  }
447  }
448  else { /* ned coordinate system not initialized, set errno */
449  errno = 3;
450  }
451  if (errno) {
452  //struct NedCoor_f _ned_zero = {0.0f};
453  //return _ned_zero;
454  }
455  /* set bit to indicate this representation is computed */
456  SetBit(state.pos_status, POS_NED_F);
457 }
458 
460  if (bit_is_set(state.pos_status, POS_ENU_F))
461  return;
462 
463  int errno = 0;
464  if (state.ned_initialized_f) {
465  if (bit_is_set(state.pos_status, POS_NED_F)) {
467  }
468  else if (bit_is_set(state.pos_status, POS_ENU_I)) {
470  }
471  else if (bit_is_set(state.pos_status, POS_NED_I)) {
473  SetBit(state.pos_status, POS_NED_F);
475  }
476  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
478  }
479  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
480  /* transform ecef_i -> enu_i -> enu_f, set status bits */
482  SetBit(state.pos_status, POS_ENU_I);
484  }
485  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
487  }
488  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
489  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
490  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
491  SetBit(state.pos_status, POS_ECEF_I);
493  SetBit(state.pos_status, POS_ENU_I);
495  }
496  else { /* could not get this representation, set errno */
497  errno = 1;
498  }
499  }
500  else if (state.utm_initialized_f) {
501  if (bit_is_set(state.pos_status, POS_ENU_I)) {
503  }
504  else if (bit_is_set(state.pos_status, POS_NED_F)) {
506  }
507  else if (bit_is_set(state.pos_status, POS_NED_I)) {
509  SetBit(state.pos_status, POS_NED_F);
511  }
512  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
514  }
515  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
516  /* transform lla_f -> utm_f -> enu, set status bits */
518  SetBit(state.pos_status, POS_UTM_F);
520  }
521  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
522  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
524  SetBit(state.pos_status, POS_LLA_F);
526  SetBit(state.pos_status, POS_UTM_F);
528  }
529  else { /* could not get this representation, set errno */
530  errno = 2;
531  }
532  }
533  else { /* ned coordinate system not initialized, set errno */
534  errno = 3;
535  }
536  if (errno) {
537  //struct EnuCoor_f _enu_zero = {0.0f};
538  //return _enu_zero;
539  }
540  /* set bit to indicate this representation is computed */
541  SetBit(state.pos_status, POS_ENU_F);
542 }
543 
545  if (bit_is_set(state.pos_status, POS_LLA_F))
546  return;
547 
548  if (bit_is_set(state.pos_status, POS_LLA_I)) {
550  }
551  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
553  }
554  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
555  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
557  SetBit(state.pos_status, POS_ECEF_F);
559  }
560  else if (bit_is_set(state.pos_status, POS_NED_F)) {
561  /* transform ned_f -> ecef_f -> lla_f, set status bits */
563  SetBit(state.pos_status, POS_ECEF_F);
565  }
566  else if (bit_is_set(state.pos_status, POS_NED_I)) {
567  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
569  SetBit(state.pos_status, POS_NED_F);
571  SetBit(state.pos_status, POS_ECEF_F);
573  }
574  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
576  }
577  else {
578  /* could not get this representation, set errno */
579  //struct LlaCoor_f _lla_zero = {0.0};
580  //return _lla_zero;
581  }
582  /* set bit to indicate this representation is computed */
583  SetBit(state.pos_status, POS_LLA_F);
584 }
591 /******************************************************************************
592  * *
593  * Transformation functions for the SPEED representations *
594  * *
595  *****************************************************************************/
598 /************************ Set functions ****************************/
599 
601  if (bit_is_set(state.speed_status, SPEED_NED_I))
602  return;
603 
604  int errno = 0;
605  if (state.ned_initialized_i) {
606  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
608  }
609  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
611  }
612  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
614  SetBit(state.speed_status, SPEED_ENU_I);
616  }
617  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
619  }
620  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
621  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
625  }
626  else { /* could not get this representation, set errno */
627  errno = 1;
628  }
629  }
630  else if (state.utm_initialized_f) {
631  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
633  }
634  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
636  }
637  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
639  SetBit(state.speed_status, SPEED_ENU_I);
641  }
642  else { /* could not get this representation, set errno */
643  errno = 2;
644  }
645  }
646  else { /* ned coordinate system not initialized, set errno */
647  errno = 3;
648  }
649  if (errno) {
650  //struct NedCoor_i _ned_zero = {0};
651  //return _ned_zero;
652  }
653  /* set bit to indicate this representation is computed */
654  SetBit(state.speed_status, SPEED_NED_I);
655 }
656 
658  if (bit_is_set(state.speed_status, SPEED_ENU_I))
659  return;
660 
661  int errno = 0;
662  if (state.ned_initialized_i) {
663  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
665  }
666  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
668  }
669  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
671  SetBit(state.pos_status, SPEED_NED_I);
673  }
674  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
676  }
677  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
678  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
682  }
683  else { /* could not get this representation, set errno */
684  errno = 1;
685  }
686  }
687  else if (state.utm_initialized_f) {
688  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
690  }
691  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
693  }
694  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
696  SetBit(state.pos_status, SPEED_NED_I);
698  }
699  else { /* could not get this representation, set errno */
700  errno = 2;
701  }
702  }
703  else { /* ned coordinate system not initialized, set errno */
704  errno = 3;
705  }
706  if (errno) {
707  //struct EnuCoor_i _enu_zero = {0};
708  //return _enu_zero;
709  }
710  /* set bit to indicate this representation is computed */
711  SetBit(state.speed_status, SPEED_ENU_I);
712 }
713 
715  if (bit_is_set(state.speed_status, SPEED_ECEF_I))
716  return;
717 
718  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
720  }
721  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
723  }
724  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
725  /* transform ned_f -> ned_i -> ecef_i , set status bits */
727  SetBit(state.speed_status, SPEED_NED_I);
729  }
730  else {
731  /* could not get this representation, set errno */
732  //struct EcefCoor_i _ecef_zero = {0};
733  //return _ecef_zero;
734  }
735  /* set bit to indicate this representation is computed */
737 }
738 
740  if (bit_is_set(state.speed_status, SPEED_HNORM_I))
741  return;
742 
743  if (bit_is_set(state.speed_status, SPEED_HNORM_F)){
745  }
746  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
750  }
751  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
755  }
756  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
760  }
761  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
765  }
766  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
767  /* transform ecef speed to ned, set status bit, then compute norm */
769  SetBit(state.speed_status, SPEED_NED_I);
773  }
774  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
776  SetBit(state.speed_status, SPEED_NED_F);
780  }
781  else {
782  //int32_t _norm_zero = 0;
783  //return _norm_zero;
784  }
785  /* set bit to indicate this representation is computed */
787 }
788 
790  if (bit_is_set(state.speed_status, SPEED_HDIR_I))
791  return;
792 
793  if (bit_is_set(state.speed_status, SPEED_HDIR_F)){
795  }
796  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
799  }
800  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
803  }
804  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
806  SetBit(state.speed_status, SPEED_NED_I);
809  }
810  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
812  SetBit(state.speed_status, SPEED_ENU_I);
815  }
816  /* set bit to indicate this representation is computed */
818 }
819 
821  if (bit_is_set(state.speed_status, SPEED_NED_F))
822  return;
823 
824  int errno = 0;
825  if (state.ned_initialized_f) {
826  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
828  }
829  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
831  }
832  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
834  SetBit(state.speed_status, SPEED_ENU_F);
836  }
837  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
839  }
840  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
841  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
845  }
846  else { /* could not get this representation, set errno */
847  errno = 1;
848  }
849  }
850  else if (state.utm_initialized_f) {
851  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
853  }
854  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
856  }
857  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
859  SetBit(state.speed_status, SPEED_ENU_F);
861  }
862  else { /* could not get this representation, set errno */
863  errno = 2;
864  }
865  }
866  else { /* ned coordinate system not initialized, set errno */
867  errno = 3;
868  }
869  if (errno) {
870  //struct NedCoor_f _ned_zero = {0.0f};
871  //return _ned_zero;
872  }
873  /* set bit to indicate this representation is computed */
874  SetBit(state.speed_status, SPEED_NED_F);
875 }
876 
878  if (bit_is_set(state.speed_status, SPEED_ENU_F))
879  return;
880 
881  int errno = 0;
882  if (state.ned_initialized_f) {
883  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
885  }
886  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
888  }
889  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
891  SetBit(state.pos_status, SPEED_NED_F);
893  }
894  else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
896  }
897  else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
898  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
902  }
903  else { /* could not get this representation, set errno */
904  errno = 1;
905  }
906  }
907  else if (state.utm_initialized_f) {
908  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
910  }
911  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
913  }
914  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
916  SetBit(state.pos_status, SPEED_NED_F);
918  }
919  else { /* could not get this representation, set errno */
920  errno = 2;
921  }
922  }
923  else { /* ned coordinate system not initialized, set errno */
924  errno = 3;
925  }
926  if (errno) {
927  //struct EnuCoor_f _enu_zero = {0};
928  //return _enu_zero;
929  }
930  /* set bit to indicate this representation is computed */
931  SetBit(state.speed_status, SPEED_ENU_F);
932 }
933 
935  if (bit_is_set(state.speed_status, SPEED_ECEF_F))
936  return;
937 
938  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
940  }
941  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
943  }
944  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
945  /* transform ned_f -> ned_i -> ecef_i , set status bits */
947  SetBit(state.speed_status, SPEED_NED_F);
949  }
950  else {
951  /* could not get this representation, set errno */
952  //struct EcefCoor_f _ecef_zero = {0.0f};
953  //return _ecef_zero;
954  }
955  /* set bit to indicate this representation is computed */
957 }
958 
960  if (bit_is_set(state.speed_status, SPEED_HNORM_F))
961  return;
962 
963  if (bit_is_set(state.speed_status, SPEED_HNORM_I)){
965  }
966  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
968  }
969  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
971  }
972  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
974  SetBit(state.speed_status, SPEED_NED_F);
976  }
977  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
979  SetBit(state.speed_status, SPEED_ENU_F);
981  }
982  /* set bit to indicate this representation is computed */
984 }
985 
987  if (bit_is_set(state.speed_status, SPEED_HDIR_F))
988  return;
989 
990  if (bit_is_set(state.speed_status, SPEED_HDIR_I)){
992  }
993  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
995  }
996  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
998  }
999  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1001  SetBit(state.speed_status, SPEED_NED_F);
1003  }
1004  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1006  SetBit(state.speed_status, SPEED_ENU_F);
1008  }
1009  /* set bit to indicate this representation is computed */
1010  SetBit(state.speed_status, SPEED_HDIR_F);
1011 }
1016 /******************************************************************************
1017  * *
1018  * Transformation functions for the ACCELERATION representations *
1019  * *
1020  *****************************************************************************/
1025  if (bit_is_set(state.accel_status, ACCEL_NED_I))
1026  return;
1027 
1028  int errno = 0;
1029  if (state.ned_initialized_i) {
1030  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1032  }
1033  else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1035  }
1036  else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1037  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1039  SetBit(state.accel_status, ACCEL_ECEF_I);
1041  }
1042  else { /* could not get this representation, set errno */
1043  errno = 1;
1044  }
1045  } else { /* ned coordinate system not initialized, set errno */
1046  errno = 2;
1047  }
1048  if (errno) {
1049  //struct NedCoor_i _ned_zero = {0};
1050  //return _ned_zero;
1051  }
1052  /* set bit to indicate this representation is computed */
1053  SetBit(state.accel_status, ACCEL_NED_I);
1054 }
1055 
1057  if (bit_is_set(state.accel_status, ACCEL_ECEF_I))
1058  return;
1059 
1060  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1062  }
1063  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1065  }
1066  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1067  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1069  SetBit(state.accel_status, ACCEL_NED_I);
1071  }
1072  else {
1073  /* could not get this representation, set errno */
1074  //struct EcefCoor_i _ecef_zero = {0};
1075  //return _ecef_zero;
1076  }
1077  /* set bit to indicate this representation is computed */
1078  SetBit(state.accel_status, ACCEL_ECEF_I);
1079 }
1080 
1082  if (bit_is_set(state.accel_status, ACCEL_NED_F))
1083  return;
1084 
1085  int errno = 0;
1086  if (state.ned_initialized_f) {
1087  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1089  }
1090  else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1092  }
1093  else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1094  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1096  SetBit(state.accel_status, ACCEL_ECEF_F);
1098  }
1099  else { /* could not get this representation, set errno */
1100  errno = 1;
1101  }
1102  } else { /* ned coordinate system not initialized, set errno */
1103  errno = 2;
1104  }
1105  if (errno) {
1106  //struct NedCoor_f _ned_zero = {0.0f};
1107  //return _ned_zero;
1108  }
1109  /* set bit to indicate this representation is computed */
1110  SetBit(state.accel_status, ACCEL_NED_F);
1111 }
1112 
1114  if (bit_is_set(state.accel_status, ACCEL_ECEF_F))
1115  return;
1116 
1117  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1119  }
1120  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1122  }
1123  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1124  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1126  SetBit(state.accel_status, ACCEL_NED_F);
1128  }
1129  else {
1130  /* could not get this representation, set errno */
1131  //struct EcefCoor_f _ecef_zero = {0.0f};
1132  //return _ecef_zero;
1133  }
1134  /* set bit to indicate this representation is computed */
1135  SetBit(state.accel_status, ACCEL_ECEF_F);
1136 }
1139 /******************************************************************************
1140  * *
1141  * Transformation functions for the ANGULAR RATE representations *
1142  * *
1143  *****************************************************************************/
1148  if (bit_is_set(state.rate_status, RATE_I))
1149  return;
1150 
1151  if (bit_is_set(state.rate_status, RATE_F)) {
1153  }
1154  /* set bit to indicate this representation is computed */
1155  SetBit(state.rate_status, RATE_I);
1156 }
1157 
1159  if (bit_is_set(state.rate_status, RATE_F))
1160  return;
1161 
1162  if (bit_is_set(state.rate_status, RATE_I)) {
1164  }
1165  /* set bit to indicate this representation is computed */
1166  SetBit(state.rate_status, RATE_F);
1167 }
1168 
1172 /******************************************************************************
1173  * *
1174  * Transformation functions for the WIND- AND AIRSPEED representations *
1175  * *
1176  *****************************************************************************/
1181  if (bit_is_set(state.wind_air_status, WINDSPEED_I))
1182  return;
1183 
1184  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1187  }
1188  /* set bit to indicate this representation is computed */
1189  SetBit(state.rate_status, WINDSPEED_I);
1190 }
1191 
1193  if (bit_is_set(state.wind_air_status, AIRSPEED_I))
1194  return;
1195 
1196  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1198  }
1199  /* set bit to indicate this representation is computed */
1200  SetBit(state.wind_air_status, AIRSPEED_I);
1201 }
1202 
1204  if (bit_is_set(state.wind_air_status, WINDSPEED_F))
1205  return;
1206 
1207  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1210  }
1211  /* set bit to indicate this representation is computed */
1212  SetBit(state.rate_status, WINDSPEED_F);
1213 }
1214 
1216  if (bit_is_set(state.wind_air_status, AIRSPEED_F))
1217  return;
1218 
1219  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1221  }
1222  /* set bit to indicate this representation is computed */
1223  SetBit(state.wind_air_status, AIRSPEED_F);
1224 }
#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:1024
#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)
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:316
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1180
float airspeed_f
Norm of relative air speed.
Definition: state.h:414
void stateCalcAirspeed_i(void)
Definition: state.c:1192
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:877
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:349
#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:1147
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)
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:739
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:820
#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:635
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:657
#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:623
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:959
#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
#define FLOAT_VECT2_NORM(n, v)
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:714
void stateCalcPositionUtm_f(void)
Definition: state.c:327
void stateCalcPositionNed_f(void)
Definition: state.c:382
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
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:789
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:1113
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:617
void stateCalcAccelNed_f(void)
Definition: state.c:1081
struct OrientationReps ned_to_body_orientation
Definition: state.h:357
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:629
void stateCalcBodyRates_f(void)
Definition: state.c:1158
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:1056
#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:934
#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.
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:373
void stateCalcPositionEnu_f(void)
Definition: state.c:459
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:986
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:641
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
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
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:1215
void stateCalcSpeedNed_i(void)
Definition: state.c:600
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:544
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:647
#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:1203