Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 {
45  state.pos_status = 0;
46  state.speed_status = 0;
47  state.accel_status = 0;
49  state.rate_status = 0;
54 }
55 
56 
57 /*******************************************************************************
58  * *
59  * transformation functions for the POSITION representations *
60  * *
61  ******************************************************************************/
66 {
67  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
68  return;
69  }
70 
71  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
73  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
75  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
76  /* transform ned_f to ecef_f, set status bit, then convert to int */
78  SetBit(state.pos_status, POS_ECEF_F);
80  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
82  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
83  /* transform lla_f to ecef_f, set status bit, then convert to int */
85  SetBit(state.pos_status, POS_ECEF_F);
87  } else {
88  /* could not get this representation, set errno */
89  //struct EcefCoor_i _ecef_zero = {0};
90  //return _ecef_zero;
91  }
92  /* set bit to indicate this representation is computed */
93  SetBit(state.pos_status, POS_ECEF_I);
94 }
95 
97 {
98  if (bit_is_set(state.pos_status, POS_NED_I)) {
99  return;
100  }
101 
102  int errno = 0;
103  if (state.ned_initialized_i) {
104  if (bit_is_set(state.pos_status, POS_NED_F)) {
106  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
108  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
110  SetBit(state.pos_status, POS_ENU_I);
112  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
114  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
115  /* transform ecef_f -> ned_f, set status bit, then convert to int */
117  SetBit(state.pos_status, POS_NED_F);
119  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
120  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
122  SetBit(state.pos_status, POS_ECEF_F);
124  SetBit(state.pos_status, POS_NED_F);
126  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
128  } else { /* could not get this representation, set errno */
129  errno = 1;
130  }
131  } else if (state.utm_initialized_f) {
132  if (bit_is_set(state.pos_status, POS_NED_F)) {
134  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
136  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
138  SetBit(state.pos_status, POS_ENU_I);
140  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
141  /* transform utm_f -> ned_f -> ned_i, set status bits */
143  SetBit(state.pos_status, POS_NED_F);
145  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
146  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
148  SetBit(state.pos_status, POS_UTM_F);
150  SetBit(state.pos_status, POS_NED_F);
152  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
153  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
155  SetBit(state.pos_status, POS_LLA_F);
157  SetBit(state.pos_status, POS_UTM_F);
159  SetBit(state.pos_status, POS_NED_F);
161  } else { /* could not get this representation, set errno */
162  errno = 2;
163  }
164  } else { /* ned coordinate system not initialized, set errno */
165  errno = 3;
166  }
167  if (errno) {
168  //struct NedCoor_i _ned_zero = {0};
169  //return _ned_zero;
170  }
171  /* set bit to indicate this representation is computed */
172  SetBit(state.pos_status, POS_NED_I);
173 }
174 
176 {
177  if (bit_is_set(state.pos_status, POS_ENU_I)) {
178  return;
179  }
180 
181  int errno = 0;
182  if (state.ned_initialized_i) {
183  if (bit_is_set(state.pos_status, POS_NED_I)) {
185  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
187  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
189  SetBit(state.pos_status, POS_NED_I);
191  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
193  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
194  /* transform ecef_f -> enu_f, set status bit, then convert to int */
196  SetBit(state.pos_status, POS_ENU_F);
198  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
199  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
201  SetBit(state.pos_status, POS_ECEF_F);
203  SetBit(state.pos_status, POS_ENU_F);
205  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
207  } else { /* could not get this representation, set errno */
208  errno = 1;
209  }
210  } else if (state.utm_initialized_f) {
211  if (bit_is_set(state.pos_status, POS_ENU_F)) {
213  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
215  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
217  SetBit(state.pos_status, POS_NED_I);
219  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
220  /* transform utm_f -> enu_f -> enu_i , set status bits */
222  SetBit(state.pos_status, POS_ENU_F);
224  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
225  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
227  SetBit(state.pos_status, POS_UTM_F);
229  SetBit(state.pos_status, POS_ENU_F);
231  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
232  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
234  SetBit(state.pos_status, POS_LLA_F);
236  SetBit(state.pos_status, POS_UTM_F);
238  SetBit(state.pos_status, POS_ENU_F);
240  } else { /* could not get this representation, set errno */
241  errno = 2;
242  }
243  } else { /* ned coordinate system not initialized, set errno */
244  errno = 3;
245  }
246  if (errno) {
247  //struct EnuCoor_i _enu_zero = {0};
248  //return _enu_zero;
249  }
250  /* set bit to indicate this representation is computed */
251  SetBit(state.pos_status, POS_ENU_I);
252 }
253 
261 {
262  if (bit_is_set(state.pos_status, POS_LLA_I)) {
263  return;
264  }
265 
266  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
268  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
269  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
271  SetBit(state.pos_status, POS_ECEF_I);
273  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
274  /* transform ned_i -> ecef_i -> lla_i, set status bits */
276  SetBit(state.pos_status, POS_ECEF_I);
278  } else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
279  /* transform enu_i -> ecef_i -> lla_i, set status bits */
281  SetBit(state.pos_status, POS_ECEF_I);
283  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
284  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
286  SetBit(state.pos_status, POS_NED_I);
288  SetBit(state.pos_status, POS_ECEF_I);
290  } else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
291  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
293  SetBit(state.pos_status, POS_ENU_I);
295  SetBit(state.pos_status, POS_ECEF_I);
297  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
299  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
300  /* transform utm_f -> lla_f -> lla_i, set status bits */
302  SetBit(state.pos_status, POS_LLA_F);
304  } else {
305  /* could not get this representation, set errno */
306  //struct LlaCoor_i _lla_zero = {0};
307  //return _lla_zero;
308  }
309  /* set bit to indicate this representation is computed */
310  SetBit(state.pos_status, POS_LLA_I);
311 }
312 
314 {
315  if (bit_is_set(state.pos_status, POS_UTM_F)) {
316  return;
317  }
318 
319  if (bit_is_set(state.pos_status, POS_LLA_F)) {
321  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
322  /* transform lla_i -> lla_f -> utm_f, set status bits */
324  SetBit(state.pos_status, POS_LLA_F);
326  } else if (state.utm_initialized_f) {
327  if (bit_is_set(state.pos_status, POS_ENU_F)) {
329  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
331  SetBit(state.pos_status, POS_ENU_F);
333  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
335  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
337  SetBit(state.pos_status, POS_NED_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 {
351  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
352  return;
353  }
354 
355  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
357  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
359  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
360  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
362  SetBit(state.pos_status, POS_ECEF_F);
364  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
366  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
368  SetBit(state.pos_status, POS_LLA_F);
370  } else {
371  /* could not get this representation, set errno */
372  //struct EcefCoor_f _ecef_zero = {0.0f};
373  //return _ecef_zero;
374  }
375  /* set bit to indicate this representation is computed */
376  SetBit(state.pos_status, POS_ECEF_F);
377 }
378 
380 {
381  if (bit_is_set(state.pos_status, POS_NED_F)) {
382  return;
383  }
384 
385  int errno = 0;
386  if (state.ned_initialized_f) {
387  if (bit_is_set(state.pos_status, POS_NED_I)) {
389  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
391  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
392  /* transform ecef_i -> ned_i -> ned_f, set status bits */
394  SetBit(state.pos_status, POS_NED_I);
396  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
398  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
399  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
400  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
401  SetBit(state.pos_status, POS_ECEF_I);
403  SetBit(state.pos_status, POS_NED_I);
405  } else { /* could not get this representation, set errno */
406  errno = 1;
407  }
408  } else if (state.utm_initialized_f) {
409  if (bit_is_set(state.pos_status, POS_NED_I)) {
411  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
413  SetBit(state.pos_status, POS_ENU_F);
415  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
417  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
419  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
420  /* transform lla_f -> utm_f -> ned, set status bits */
422  SetBit(state.pos_status, POS_UTM_F);
424  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
425  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
427  SetBit(state.pos_status, POS_LLA_F);
429  SetBit(state.pos_status, POS_UTM_F);
431  } else { /* could not get this representation, set errno */
432  errno = 2;
433  }
434  } else { /* ned coordinate system not initialized, set errno */
435  errno = 3;
436  }
437  if (errno) {
438  //struct NedCoor_f _ned_zero = {0.0f};
439  //return _ned_zero;
440  }
441  /* set bit to indicate this representation is computed */
442  SetBit(state.pos_status, POS_NED_F);
443 }
444 
446 {
447  if (bit_is_set(state.pos_status, POS_ENU_F)) {
448  return;
449  }
450 
451  int errno = 0;
452  if (state.ned_initialized_f) {
453  if (bit_is_set(state.pos_status, POS_NED_F)) {
455  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
457  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
459  SetBit(state.pos_status, POS_NED_F);
461  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
463  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
464  /* transform ecef_i -> enu_i -> enu_f, set status bits */
466  SetBit(state.pos_status, POS_ENU_I);
468  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
470  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
471  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
472  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
473  SetBit(state.pos_status, POS_ECEF_I);
475  SetBit(state.pos_status, POS_ENU_I);
477  } else { /* could not get this representation, set errno */
478  errno = 1;
479  }
480  } else if (state.utm_initialized_f) {
481  if (bit_is_set(state.pos_status, POS_ENU_I)) {
483  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
485  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
487  SetBit(state.pos_status, POS_NED_F);
489  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
491  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
492  /* transform lla_f -> utm_f -> enu, set status bits */
494  SetBit(state.pos_status, POS_UTM_F);
496  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
497  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
499  SetBit(state.pos_status, POS_LLA_F);
501  SetBit(state.pos_status, POS_UTM_F);
503  } else { /* could not get this representation, set errno */
504  errno = 2;
505  }
506  } else { /* ned coordinate system not initialized, set errno */
507  errno = 3;
508  }
509  if (errno) {
510  //struct EnuCoor_f _enu_zero = {0.0f};
511  //return _enu_zero;
512  }
513  /* set bit to indicate this representation is computed */
514  SetBit(state.pos_status, POS_ENU_F);
515 }
516 
518 {
519  if (bit_is_set(state.pos_status, POS_LLA_F)) {
520  return;
521  }
522 
523  if (bit_is_set(state.pos_status, POS_LLA_I)) {
525  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
527  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
528  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
530  SetBit(state.pos_status, POS_ECEF_F);
532  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
533  /* transform ned_f -> ecef_f -> lla_f, set status bits */
535  SetBit(state.pos_status, POS_ECEF_F);
537  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_f) {
538  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
540  SetBit(state.pos_status, POS_NED_F);
542  SetBit(state.pos_status, POS_ECEF_F);
544  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
546  } else {
547  /* could not get this representation, set errno */
548  //struct LlaCoor_f _lla_zero = {0.0};
549  //return _lla_zero;
550  }
551  /* set bit to indicate this representation is computed */
552  SetBit(state.pos_status, POS_LLA_F);
553 }
560 /******************************************************************************
561  * *
562  * Transformation functions for the SPEED representations *
563  * *
564  *****************************************************************************/
567 /************************ Set functions ****************************/
568 
570 {
571  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
572  return;
573  }
574 
575  int errno = 0;
576  if (state.ned_initialized_i) {
577  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
579  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
581  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
583  SetBit(state.speed_status, SPEED_ENU_I);
585  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
587  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
588  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
592  } else { /* could not get this representation, set errno */
593  errno = 1;
594  }
595  } else if (state.utm_initialized_f) {
596  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
598  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
600  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
602  SetBit(state.speed_status, SPEED_ENU_I);
604  } else { /* could not get this representation, set errno */
605  errno = 2;
606  }
607  } else { /* ned coordinate system not initialized, set errno */
608  errno = 3;
609  }
610  if (errno) {
611  //struct NedCoor_i _ned_zero = {0};
612  //return _ned_zero;
613  }
614  /* set bit to indicate this representation is computed */
615  SetBit(state.speed_status, SPEED_NED_I);
616 }
617 
619 {
620  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
621  return;
622  }
623 
624  int errno = 0;
625  if (state.ned_initialized_i) {
626  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
628  }
629  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
631  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
633  SetBit(state.pos_status, SPEED_NED_I);
635  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
637  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
638  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
642  } else { /* could not get this representation, set errno */
643  errno = 1;
644  }
645  } else if (state.utm_initialized_f) {
646  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
648  }
649  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
651  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
653  SetBit(state.pos_status, SPEED_NED_I);
655  } else { /* could not get this representation, set errno */
656  errno = 2;
657  }
658  } else { /* ned coordinate system not initialized, set errno */
659  errno = 3;
660  }
661  if (errno) {
662  //struct EnuCoor_i _enu_zero = {0};
663  //return _enu_zero;
664  }
665  /* set bit to indicate this representation is computed */
666  SetBit(state.speed_status, SPEED_ENU_I);
667 }
668 
670 {
671  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
672  return;
673  }
674 
675  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
677  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
679  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
680  /* transform ned_f -> ned_i -> ecef_i , set status bits */
682  SetBit(state.speed_status, SPEED_NED_I);
684  } else {
685  /* could not get this representation, set errno */
686  //struct EcefCoor_i _ecef_zero = {0};
687  //return _ecef_zero;
688  }
689  /* set bit to indicate this representation is computed */
691 }
692 
694 {
695  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
696  return;
697  }
698 
699  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
701  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
705  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
709  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
713  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
717  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
718  /* transform ecef speed to ned, set status bit, then compute norm */
720  SetBit(state.speed_status, SPEED_NED_I);
724  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
726  SetBit(state.speed_status, SPEED_NED_F);
730  } else {
731  //int32_t _norm_zero = 0;
732  //return _norm_zero;
733  }
734  /* set bit to indicate this representation is computed */
736 }
737 
739 {
740  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
741  return;
742  }
743 
744  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
746  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
749  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
752  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
754  SetBit(state.speed_status, SPEED_NED_I);
757  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
759  SetBit(state.speed_status, SPEED_ENU_I);
762  }
763  /* set bit to indicate this representation is computed */
765 }
766 
768 {
769  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
770  return;
771  }
772 
773  int errno = 0;
774  if (state.ned_initialized_f) {
775  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
777  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
779  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
781  SetBit(state.speed_status, SPEED_ENU_F);
783  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
785  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
786  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
790  } else { /* could not get this representation, set errno */
791  errno = 1;
792  }
793  } else if (state.utm_initialized_f) {
794  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
796  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
798  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
800  SetBit(state.speed_status, SPEED_ENU_F);
802  } else { /* could not get this representation, set errno */
803  errno = 2;
804  }
805  } else { /* ned coordinate system not initialized, set errno */
806  errno = 3;
807  }
808  if (errno) {
809  //struct NedCoor_f _ned_zero = {0.0f};
810  //return _ned_zero;
811  }
812  /* set bit to indicate this representation is computed */
813  SetBit(state.speed_status, SPEED_NED_F);
814 }
815 
817 {
818  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
819  return;
820  }
821 
822  int errno = 0;
823  if (state.ned_initialized_f) {
824  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
826  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
828  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
830  SetBit(state.pos_status, SPEED_NED_F);
832  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
834  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
835  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
839  } else { /* could not get this representation, set errno */
840  errno = 1;
841  }
842  } else if (state.utm_initialized_f) {
843  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
845  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
847  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
849  SetBit(state.pos_status, SPEED_NED_F);
851  } else { /* could not get this representation, set errno */
852  errno = 2;
853  }
854  } else { /* ned coordinate system not initialized, set errno */
855  errno = 3;
856  }
857  if (errno) {
858  //struct EnuCoor_f _enu_zero = {0};
859  //return _enu_zero;
860  }
861  /* set bit to indicate this representation is computed */
862  SetBit(state.speed_status, SPEED_ENU_F);
863 }
864 
866 {
867  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
868  return;
869  }
870 
871  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
873  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
875  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
876  /* transform ned_f -> ned_i -> ecef_i , set status bits */
878  SetBit(state.speed_status, SPEED_NED_F);
880  } else {
881  /* could not get this representation, set errno */
882  //struct EcefCoor_f _ecef_zero = {0.0f};
883  //return _ecef_zero;
884  }
885  /* set bit to indicate this representation is computed */
887 }
888 
890 {
891  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
892  return;
893  }
894 
895  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
897  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
899  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
901  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
903  SetBit(state.speed_status, SPEED_NED_F);
905  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
907  SetBit(state.speed_status, SPEED_ENU_F);
909  }
910  /* set bit to indicate this representation is computed */
912 }
913 
915 {
916  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
917  return;
918  }
919 
920  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
922  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
924  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
926  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
928  SetBit(state.speed_status, SPEED_NED_F);
930  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
932  SetBit(state.speed_status, SPEED_ENU_F);
934  }
935  /* set bit to indicate this representation is computed */
937 }
942 /******************************************************************************
943  * *
944  * Transformation functions for the ACCELERATION representations *
945  * *
946  *****************************************************************************/
951 {
952  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
953  return;
954  }
955 
956  int errno = 0;
957  if (state.ned_initialized_i) {
958  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
960  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
962  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
963  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
967  } else { /* could not get this representation, set errno */
968  errno = 1;
969  }
970  } else { /* ned coordinate system not initialized, set errno */
971  errno = 2;
972  }
973  if (errno) {
974  //struct NedCoor_i _ned_zero = {0};
975  //return _ned_zero;
976  }
977  /* set bit to indicate this representation is computed */
978  SetBit(state.accel_status, ACCEL_NED_I);
979 }
980 
982 {
983  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
984  return;
985  }
986 
987  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
989  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
991  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
992  /* transform ned_f -> ned_i -> ecef_i , set status bits */
994  SetBit(state.accel_status, ACCEL_NED_I);
996  } else {
997  /* could not get this representation, set errno */
998  //struct EcefCoor_i _ecef_zero = {0};
999  //return _ecef_zero;
1000  }
1001  /* set bit to indicate this representation is computed */
1002  SetBit(state.accel_status, ACCEL_ECEF_I);
1003 }
1004 
1006 {
1007  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1008  return;
1009  }
1010 
1011  int errno = 0;
1012  if (state.ned_initialized_f) {
1013  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1015  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1017  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1018  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1020  SetBit(state.accel_status, ACCEL_ECEF_F);
1022  } else { /* could not get this representation, set errno */
1023  errno = 1;
1024  }
1025  } else { /* ned coordinate system not initialized, set errno */
1026  errno = 2;
1027  }
1028  if (errno) {
1029  //struct NedCoor_f _ned_zero = {0.0f};
1030  //return _ned_zero;
1031  }
1032  /* set bit to indicate this representation is computed */
1033  SetBit(state.accel_status, ACCEL_NED_F);
1034 }
1035 
1037 {
1038  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1039  return;
1040  }
1041 
1042  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1044  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1046  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1047  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1049  SetBit(state.accel_status, ACCEL_NED_F);
1051  } else {
1052  /* could not get this representation, set errno */
1053  //struct EcefCoor_f _ecef_zero = {0.0f};
1054  //return _ecef_zero;
1055  }
1056  /* set bit to indicate this representation is computed */
1057  SetBit(state.accel_status, ACCEL_ECEF_F);
1058 }
1061 /******************************************************************************
1062  * *
1063  * Transformation functions for the ANGULAR RATE representations *
1064  * *
1065  *****************************************************************************/
1070 {
1071  if (bit_is_set(state.rate_status, RATE_I)) {
1072  return;
1073  }
1074 
1075  if (bit_is_set(state.rate_status, RATE_F)) {
1077  }
1078  /* set bit to indicate this representation is computed */
1079  SetBit(state.rate_status, RATE_I);
1080 }
1081 
1083 {
1084  if (bit_is_set(state.rate_status, RATE_F)) {
1085  return;
1086  }
1087 
1088  if (bit_is_set(state.rate_status, RATE_I)) {
1090  }
1091  /* set bit to indicate this representation is computed */
1092  SetBit(state.rate_status, RATE_F);
1093 }
1094 
1098 /******************************************************************************
1099  * *
1100  * Transformation functions for the WIND- AND AIRSPEED representations *
1101  * *
1102  *****************************************************************************/
1107 {
1108  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1109  return;
1110  }
1111 
1112  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1115  }
1116  /* set bit to indicate this representation is computed */
1117  SetBit(state.rate_status, WINDSPEED_I);
1118 }
1119 
1121 {
1122  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1123  return;
1124  }
1125 
1126  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1128  }
1129  /* set bit to indicate this representation is computed */
1130  SetBit(state.wind_air_status, AIRSPEED_I);
1131 }
1132 
1134 {
1135  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1136  return;
1137  }
1138 
1139  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1142  }
1143  /* set bit to indicate this representation is computed */
1144  SetBit(state.rate_status, WINDSPEED_F);
1145 }
1146 
1148 {
1149  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1150  return;
1151  }
1152 
1153  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1155  }
1156  /* set bit to indicate this representation is computed */
1157  SetBit(state.wind_air_status, AIRSPEED_F);
1158 }
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 POS_ENU_F
Definition: state.h:72
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:722
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:274
#define ACCEL_ECEF_I
Definition: state.h:100
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:698
#define ECEF_BFP_OF_REAL(_o, _i)
#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)
float y
in meters
void stateCalcAccelNed_i(void)
Definition: state.c:950
int32_t y
North.
int32_t x
East.
#define AIRSPEED_F
Definition: state.h:121
#define WINDSPEED_I
Definition: state.h:118
#define INT32_SPEED_FRAC
struct FloatVect2 h_windspeed_f
Horizontal windspeed.
Definition: state.h:408
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:316
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1106
float airspeed_f
Norm of relative air speed.
Definition: state.h:414
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.
void stateCalcAirspeed_i(void)
Definition: state.c:1120
#define NED_FLOAT_OF_BFP(_o, _i)
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
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)
#define SPEED_ENU_F
Definition: state.h:90
void stateCalcSpeedEnu_f(void)
Definition: state.c:816
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
#define POS_NED_F
Definition: state.h:71
void stateCalcPositionEcef_f(void)
Definition: state.c:349
#define SPEED_FLOAT_OF_BFP(_ai)
struct Int32Vect2 h_windspeed_i
Horizontal windspeed in north/east.
Definition: state.h:396
void stateCalcBodyRates_i(void)
Definition: state.c:1069
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:287
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:716
#define POS_NED_I
Definition: state.h:66
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:693
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
#define NED_BFP_OF_REAL(_o, _i)
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 stateCalcSpeedNed_f(void)
Definition: state.c:767
#define FALSE
Definition: std.h:5
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:734
int32_t y
East.
#define INT32_VECT3_NED_OF_ENU(_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
void stateCalcSpeedEnu_i(void)
Definition: state.c:618
#define FLOAT_VECT2_NORM(_v)
#define ACCEL_ECEF_F
Definition: state.h:102
#define INT32_SQRT(_out, _in)
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:256
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:692
#define SPEED_HNORM_I
Definition: state.h:86
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.
float x
in meters
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:889
#define SPEED_HDIR_I
Definition: state.h:87
#define AIRSPEED_I
Definition: state.h:119
void stateCalcPositionEnu_i(void)
Definition: state.c:175
#define SPEED_ECEF_I
Definition: state.h:83
int32_t x
North.
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
unsigned long uint32_t
Definition: types.h:18
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:260
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:327
#define ENU_FLOAT_OF_BFP(_o, _i)
void stateCalcSpeedEcef_i(void)
Definition: state.c:669
void stateCalcPositionUtm_f(void)
Definition: state.c:313
void stateCalcPositionNed_f(void)
Definition: state.c:379
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:92
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:738
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:304
void stateCalcAccelEcef_f(void)
Definition: state.c:1036
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
void stateCalcAccelNed_f(void)
Definition: state.c:1005
#define VECT3_ENU_OF_NED(_o, _i)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:728
void stateCalcBodyRates_f(void)
Definition: state.c:1082
#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:981
#define VECT3_NED_OF_ENU(_o, _i)
uint8_t status
Holds the status bits for all orientation representations.
int32_t airspeed_i
Norm of relative wind 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:865
#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
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 ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
API to get/set the generic vehicle states.
Structure holding vehicle state data.
Definition: state.h:130
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:373
void stateCalcPositionEnu_f(void)
Definition: state.c:445
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:390
#define INT32_COURSE_NORMALIZE(_a)
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:914
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:345
#define INT32_VECT3_ENU_OF_NED(_o, _i)
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.
#define LLA_BFP_OF_REAL(_o, _i)
#define SPEED_NED_F
Definition: state.h:89
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:188
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:280
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:293
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:262
#define ENU_BFP_OF_REAL(_o, _i)
void stateCalcPositionNed_i(void)
Definition: state.c:96
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
void stateCalcAirspeed_f(void)
Definition: state.c:1147
void stateCalcSpeedNed_i(void)
Definition: state.c:569
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 SPEED_BFP_OF_REAL(_af)
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:140
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
#define SPEED_HDIR_F
Definition: state.h:92
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
#define ECEF_FLOAT_OF_BFP(_o, _i)
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
void stateCalcPositionEcef_i(void)
Definition: state.c:65
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:245
void stateCalcPositionLla_f(void)
Definition: state.c:517
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:351
struct OrientationReps ned_to_body_orientation
Definition: state.h:357
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
#define POS_ECEF_F
Definition: state.h:70
float y
in meters
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:74
#define SPEED_ECEF_F
Definition: state.h:88
#define RATE_I
Definition: state.h:110
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:86
#define ACCEL_NED_I
Definition: state.h:101
#define POS_ENU_I
Definition: state.h:67
#define LLA_FLOAT_OF_BFP(_o, _i)
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:174
#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)
int32_t int32_atan2(int32_t y, int32_t x)
float x
in meters
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1133
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:80
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.