Paparazzi UAS  v5.12_stable-4-g9b43e9b
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;
51  state.ned_initialized_i = false;
52  state.ned_initialized_f = false;
53  state.utm_initialized_f = false;
54 
55  /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
57 }
58 
59 
60 /*******************************************************************************
61  * *
62  * transformation functions for the POSITION representations *
63  * *
64  ******************************************************************************/
69 {
70  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
71  return;
72  }
73 
74  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
76  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
78  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
79  /* transform ned_f to ecef_f, set status bit, then convert to int */
81  SetBit(state.pos_status, POS_ECEF_F);
83  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
85  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
86  /* transform lla_f to ecef_f, set status bit, then convert to int */
88  SetBit(state.pos_status, POS_ECEF_F);
90  } else {
91  /* could not get this representation, set errno */
92  //struct EcefCoor_i _ecef_zero = {0};
93  //return _ecef_zero;
94  }
95  /* set bit to indicate this representation is computed */
96  SetBit(state.pos_status, POS_ECEF_I);
97 }
98 
100 {
101  if (bit_is_set(state.pos_status, POS_NED_I)) {
102  return;
103  }
104 
105  int errno = 0;
106  if (state.ned_initialized_i) {
107  if (bit_is_set(state.pos_status, POS_NED_F)) {
109  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
111  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
113  SetBit(state.pos_status, POS_ENU_I);
115  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
117  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
118  /* transform ecef_f -> ned_f, set status bit, then convert to int */
120  SetBit(state.pos_status, POS_NED_F);
122  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
123  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
125  SetBit(state.pos_status, POS_ECEF_F);
127  SetBit(state.pos_status, POS_NED_F);
129  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
131  } else { /* could not get this representation, set errno */
132  errno = 1;
133  }
134  } else if (state.utm_initialized_f) {
135  if (bit_is_set(state.pos_status, POS_NED_F)) {
137  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
139  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
141  SetBit(state.pos_status, POS_ENU_I);
143  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
144  /* transform utm_f -> ned_f -> ned_i, set status bits */
146  SetBit(state.pos_status, POS_NED_F);
148  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
149  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
151  SetBit(state.pos_status, POS_UTM_F);
153  SetBit(state.pos_status, POS_NED_F);
155  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
156  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
158  SetBit(state.pos_status, POS_LLA_F);
160  SetBit(state.pos_status, POS_UTM_F);
162  SetBit(state.pos_status, POS_NED_F);
164  } else { /* could not get this representation, set errno */
165  errno = 2;
166  }
167  } else { /* ned coordinate system not initialized, set errno */
168  errno = 3;
169  }
170  if (errno) {
171  //struct NedCoor_i _ned_zero = {0};
172  //return _ned_zero;
173  }
174  /* set bit to indicate this representation is computed */
175  SetBit(state.pos_status, POS_NED_I);
176 }
177 
179 {
180  if (bit_is_set(state.pos_status, POS_ENU_I)) {
181  return;
182  }
183 
184  int errno = 0;
185  if (state.ned_initialized_i) {
186  if (bit_is_set(state.pos_status, POS_NED_I)) {
188  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
190  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
192  SetBit(state.pos_status, POS_NED_I);
194  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
196  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
197  /* transform ecef_f -> enu_f, set status bit, then convert to int */
199  SetBit(state.pos_status, POS_ENU_F);
201  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
202  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
204  SetBit(state.pos_status, POS_ECEF_F);
206  SetBit(state.pos_status, POS_ENU_F);
208  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
210  } else { /* could not get this representation, set errno */
211  errno = 1;
212  }
213  } else if (state.utm_initialized_f) {
214  if (bit_is_set(state.pos_status, POS_ENU_F)) {
216  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
218  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
220  SetBit(state.pos_status, POS_NED_I);
222  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
223  /* transform utm_f -> enu_f -> enu_i , set status bits */
225  SetBit(state.pos_status, POS_ENU_F);
227  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
228  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
230  SetBit(state.pos_status, POS_UTM_F);
232  SetBit(state.pos_status, POS_ENU_F);
234  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
235  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
237  SetBit(state.pos_status, POS_LLA_F);
239  SetBit(state.pos_status, POS_UTM_F);
241  SetBit(state.pos_status, POS_ENU_F);
243  } else { /* could not get this representation, set errno */
244  errno = 2;
245  }
246  } else { /* ned coordinate system not initialized, set errno */
247  errno = 3;
248  }
249  if (errno) {
250  //struct EnuCoor_i _enu_zero = {0};
251  //return _enu_zero;
252  }
253  /* set bit to indicate this representation is computed */
254  SetBit(state.pos_status, POS_ENU_I);
255 }
256 
264 {
265  if (bit_is_set(state.pos_status, POS_LLA_I)) {
266  return;
267  }
268 
269  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
271  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
272  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
274  SetBit(state.pos_status, POS_ECEF_I);
276  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
277  /* transform ned_i -> ecef_i -> lla_i, set status bits */
279  SetBit(state.pos_status, POS_ECEF_I);
281  } else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
282  /* transform enu_i -> ecef_i -> lla_i, set status bits */
284  SetBit(state.pos_status, POS_ECEF_I);
286  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
287  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
289  SetBit(state.pos_status, POS_NED_I);
291  SetBit(state.pos_status, POS_ECEF_I);
293  } else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
294  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
296  SetBit(state.pos_status, POS_ENU_I);
298  SetBit(state.pos_status, POS_ECEF_I);
300  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
302  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
303  /* transform utm_f -> lla_f -> lla_i, set status bits */
305  SetBit(state.pos_status, POS_LLA_F);
307  } else {
308  /* could not get this representation, set errno */
309  //struct LlaCoor_i _lla_zero = {0};
310  //return _lla_zero;
311  }
312  /* set bit to indicate this representation is computed */
313  SetBit(state.pos_status, POS_LLA_I);
314 }
315 
317 {
318  if (bit_is_set(state.pos_status, POS_UTM_F)) {
319  return;
320  }
321 
322  if (bit_is_set(state.pos_status, POS_LLA_F)) {
324  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
325  /* transform lla_i -> lla_f -> utm_f, set status bits */
327  SetBit(state.pos_status, POS_LLA_F);
329  } else if (state.utm_initialized_f) {
330  if (bit_is_set(state.pos_status, POS_ENU_F)) {
332  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
334  SetBit(state.pos_status, POS_ENU_F);
336  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
338  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
340  SetBit(state.pos_status, POS_NED_F);
342  }
343  } else {
344  /* could not get this representation, set errno */
345  //struct EcefCoor_f _ecef_zero = {0.0f};
346  //return _ecef_zero;
347  }
348  /* set bit to indicate this representation is computed */
349  SetBit(state.pos_status, POS_UTM_F);
350 }
351 
353 {
354  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
355  return;
356  }
357 
358  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
360  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
362  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
363  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
365  SetBit(state.pos_status, POS_ECEF_F);
367  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
369  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
371  SetBit(state.pos_status, POS_LLA_F);
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 {
384  if (bit_is_set(state.pos_status, POS_NED_F)) {
385  return;
386  }
387 
388  int errno = 0;
389  if (state.ned_initialized_f) {
390  if (bit_is_set(state.pos_status, POS_NED_I)) {
392  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
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  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
401  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
402  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
403  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
404  SetBit(state.pos_status, POS_ECEF_I);
406  SetBit(state.pos_status, POS_NED_I);
408  } else { /* could not get this representation, set errno */
409  errno = 1;
410  }
411  } else if (state.utm_initialized_f) {
412  if (bit_is_set(state.pos_status, POS_NED_I)) {
414  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
416  SetBit(state.pos_status, POS_ENU_F);
418  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
420  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
422  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
423  /* transform lla_f -> utm_f -> ned, set status bits */
425  SetBit(state.pos_status, POS_UTM_F);
427  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
428  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
430  SetBit(state.pos_status, POS_LLA_F);
432  SetBit(state.pos_status, POS_UTM_F);
434  } else { /* could not get this representation, set errno */
435  errno = 2;
436  }
437  } else { /* ned coordinate system not initialized, set errno */
438  errno = 3;
439  }
440  if (errno) {
441  //struct NedCoor_f _ned_zero = {0.0f};
442  //return _ned_zero;
443  }
444  /* set bit to indicate this representation is computed */
445  SetBit(state.pos_status, POS_NED_F);
446 }
447 
449 {
450  if (bit_is_set(state.pos_status, POS_ENU_F)) {
451  return;
452  }
453 
454  int errno = 0;
455  if (state.ned_initialized_f) {
456  if (bit_is_set(state.pos_status, POS_NED_F)) {
458  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
460  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
462  SetBit(state.pos_status, POS_NED_F);
464  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
466  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
467  /* transform ecef_i -> enu_i -> enu_f, set status bits */
469  SetBit(state.pos_status, POS_ENU_I);
471  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
473  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
474  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
475  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
476  SetBit(state.pos_status, POS_ECEF_I);
478  SetBit(state.pos_status, POS_ENU_I);
480  } else { /* could not get this representation, set errno */
481  errno = 1;
482  }
483  } else if (state.utm_initialized_f) {
484  if (bit_is_set(state.pos_status, POS_ENU_I)) {
486  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
488  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
490  SetBit(state.pos_status, POS_NED_F);
492  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
494  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
495  /* transform lla_f -> utm_f -> enu, set status bits */
497  SetBit(state.pos_status, POS_UTM_F);
499  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
500  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
502  SetBit(state.pos_status, POS_LLA_F);
504  SetBit(state.pos_status, POS_UTM_F);
506  } else { /* could not get this representation, set errno */
507  errno = 2;
508  }
509  } else { /* ned coordinate system not initialized, set errno */
510  errno = 3;
511  }
512  if (errno) {
513  //struct EnuCoor_f _enu_zero = {0.0f};
514  //return _enu_zero;
515  }
516  /* set bit to indicate this representation is computed */
517  SetBit(state.pos_status, POS_ENU_F);
518 }
519 
521 {
522  if (bit_is_set(state.pos_status, POS_LLA_F)) {
523  return;
524  }
525 
526  if (bit_is_set(state.pos_status, POS_LLA_I)) {
528  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
530  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
531  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
533  SetBit(state.pos_status, POS_ECEF_F);
535  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
536  /* transform ned_f -> ecef_f -> lla_f, set status bits */
538  SetBit(state.pos_status, POS_ECEF_F);
540  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_f) {
541  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
543  SetBit(state.pos_status, POS_NED_F);
545  SetBit(state.pos_status, POS_ECEF_F);
547  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
549  } else {
550  /* could not get this representation, set errno */
551  //struct LlaCoor_f _lla_zero = {0.0};
552  //return _lla_zero;
553  }
554  /* set bit to indicate this representation is computed */
555  SetBit(state.pos_status, POS_LLA_F);
556 }
563 /******************************************************************************
564  * *
565  * Transformation functions for the SPEED representations *
566  * *
567  *****************************************************************************/
570 /************************ Set functions ****************************/
571 
573 {
574  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
575  return;
576  }
577 
578  int errno = 0;
579  if (state.ned_initialized_i) {
580  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
582  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
584  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
586  SetBit(state.speed_status, SPEED_ENU_I);
588  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
590  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
591  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
595  } else { /* could not get this representation, set errno */
596  errno = 1;
597  }
598  } else if (state.utm_initialized_f) {
599  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
601  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
603  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
605  SetBit(state.speed_status, SPEED_ENU_I);
607  } else { /* could not get this representation, set errno */
608  errno = 2;
609  }
610  } else { /* ned coordinate system not initialized, set errno */
611  errno = 3;
612  }
613  if (errno) {
614  //struct NedCoor_i _ned_zero = {0};
615  //return _ned_zero;
616  }
617  /* set bit to indicate this representation is computed */
618  SetBit(state.speed_status, SPEED_NED_I);
619 }
620 
622 {
623  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
624  return;
625  }
626 
627  int errno = 0;
628  if (state.ned_initialized_i) {
629  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
631  }
632  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
634  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
636  SetBit(state.pos_status, SPEED_NED_I);
638  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
640  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
641  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
645  } else { /* could not get this representation, set errno */
646  errno = 1;
647  }
648  } else if (state.utm_initialized_f) {
649  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
651  }
652  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
654  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
656  SetBit(state.pos_status, SPEED_NED_I);
658  } else { /* could not get this representation, set errno */
659  errno = 2;
660  }
661  } else { /* ned coordinate system not initialized, set errno */
662  errno = 3;
663  }
664  if (errno) {
665  //struct EnuCoor_i _enu_zero = {0};
666  //return _enu_zero;
667  }
668  /* set bit to indicate this representation is computed */
669  SetBit(state.speed_status, SPEED_ENU_I);
670 }
671 
673 {
674  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
675  return;
676  }
677 
678  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
680  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
682  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
683  /* transform ned_f -> ned_i -> ecef_i , set status bits */
685  SetBit(state.speed_status, SPEED_NED_I);
687  } else {
688  /* could not get this representation, set errno */
689  //struct EcefCoor_i _ecef_zero = {0};
690  //return _ecef_zero;
691  }
692  /* set bit to indicate this representation is computed */
694 }
695 
697 {
698  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
699  return;
700  }
701 
702  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
704  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
708  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
712  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
716  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
720  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
721  /* transform ecef speed to ned, set status bit, then compute norm */
723  SetBit(state.speed_status, SPEED_NED_I);
727  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
729  SetBit(state.speed_status, SPEED_NED_F);
733  } else {
734  //int32_t _norm_zero = 0;
735  //return _norm_zero;
736  }
737  /* set bit to indicate this representation is computed */
739 }
740 
742 {
743  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
744  return;
745  }
746 
747  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
749  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
752  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
755  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
757  SetBit(state.speed_status, SPEED_NED_I);
760  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
762  SetBit(state.speed_status, SPEED_ENU_I);
765  }
766  /* set bit to indicate this representation is computed */
768 }
769 
771 {
772  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
773  return;
774  }
775 
776  int errno = 0;
777  if (state.ned_initialized_f) {
778  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
780  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
782  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
784  SetBit(state.speed_status, SPEED_ENU_F);
786  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
788  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
789  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
793  } else { /* could not get this representation, set errno */
794  errno = 1;
795  }
796  } else if (state.utm_initialized_f) {
797  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
799  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
801  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
803  SetBit(state.speed_status, SPEED_ENU_F);
805  } else { /* could not get this representation, set errno */
806  errno = 2;
807  }
808  } else { /* ned coordinate system not initialized, set errno */
809  errno = 3;
810  }
811  if (errno) {
812  //struct NedCoor_f _ned_zero = {0.0f};
813  //return _ned_zero;
814  }
815  /* set bit to indicate this representation is computed */
816  SetBit(state.speed_status, SPEED_NED_F);
817 }
818 
820 {
821  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
822  return;
823  }
824 
825  int errno = 0;
826  if (state.ned_initialized_f) {
827  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
829  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
831  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
833  SetBit(state.pos_status, SPEED_NED_F);
835  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
837  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
838  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
842  } else { /* could not get this representation, set errno */
843  errno = 1;
844  }
845  } else if (state.utm_initialized_f) {
846  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
848  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
850  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
852  SetBit(state.pos_status, SPEED_NED_F);
854  } else { /* could not get this representation, set errno */
855  errno = 2;
856  }
857  } else { /* ned coordinate system not initialized, set errno */
858  errno = 3;
859  }
860  if (errno) {
861  //struct EnuCoor_f _enu_zero = {0};
862  //return _enu_zero;
863  }
864  /* set bit to indicate this representation is computed */
865  SetBit(state.speed_status, SPEED_ENU_F);
866 }
867 
869 {
870  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
871  return;
872  }
873 
874  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
876  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
878  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
879  /* transform ned_f -> ned_i -> ecef_i , set status bits */
881  SetBit(state.speed_status, SPEED_NED_F);
883  } else {
884  /* could not get this representation, set errno */
885  //struct EcefCoor_f _ecef_zero = {0.0f};
886  //return _ecef_zero;
887  }
888  /* set bit to indicate this representation is computed */
890 }
891 
893 {
894  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
895  return;
896  }
897 
898  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
900  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
902  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
904  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
906  SetBit(state.speed_status, SPEED_NED_F);
908  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
910  SetBit(state.speed_status, SPEED_ENU_F);
912  }
913  /* set bit to indicate this representation is computed */
915 }
916 
918 {
919  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
920  return;
921  }
922 
923  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
925  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
927  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
929  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
931  SetBit(state.speed_status, SPEED_NED_F);
933  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
935  SetBit(state.speed_status, SPEED_ENU_F);
937  }
938  /* set bit to indicate this representation is computed */
940 }
945 /******************************************************************************
946  * *
947  * Transformation functions for the ACCELERATION representations *
948  * *
949  *****************************************************************************/
954 {
955  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
956  return;
957  }
958 
959  int errno = 0;
960  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
962  }
963  else if (state.ned_initialized_i) {
964  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
966  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
967  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
971  } else { /* could not get this representation, set errno */
972  errno = 1;
973  }
974  } else { /* ned coordinate system not initialized, set errno */
975  errno = 2;
976  }
977  if (errno) {
978  //struct NedCoor_i _ned_zero = {0};
979  //return _ned_zero;
980  }
981  /* set bit to indicate this representation is computed */
982  SetBit(state.accel_status, ACCEL_NED_I);
983 }
984 
986 {
987  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
988  return;
989  }
990 
991  int errno = 0;
992  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
994  }
995  else if (state.ned_initialized_i) {
996  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
998  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
999  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1001  SetBit(state.accel_status, ACCEL_NED_I);
1003  } else {
1004  /* could not get this representation, set errno */
1005  errno = 1;
1006  }
1007  } else { /* ned coordinate system not initialized, set errno */
1008  errno = 2;
1009  }
1010  if (errno) {}
1011  /* set bit to indicate this representation is computed */
1012  SetBit(state.accel_status, ACCEL_ECEF_I);
1013 }
1014 
1016 {
1017  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1018  return;
1019  }
1020 
1021  int errno = 0;
1022  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1024  }
1025  else if (state.ned_initialized_f) {
1026  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1028  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1029  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1031  SetBit(state.accel_status, ACCEL_ECEF_F);
1033  } else { /* could not get this representation, set errno */
1034  errno = 1;
1035  }
1036  } else { /* ned coordinate system not initialized, set errno */
1037  errno = 2;
1038  }
1039  if (errno) {
1040  //struct NedCoor_f _ned_zero = {0.0f};
1041  //return _ned_zero;
1042  }
1043  /* set bit to indicate this representation is computed */
1044  SetBit(state.accel_status, ACCEL_NED_F);
1045 }
1046 
1048 {
1049  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1050  return;
1051  }
1052 
1053  int errno = 0;
1054  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1056  }
1057  else if (state.ned_initialized_f) {
1058  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1060  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1061  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1063  SetBit(state.accel_status, ACCEL_NED_F);
1065  } else {
1066  /* could not get this representation, set errno */
1067  errno = 1;
1068  }
1069  } else { /* ned coordinate system not initialized, set errno */
1070  errno = 2;
1071  }
1072  if (errno) {}
1073  /* set bit to indicate this representation is computed */
1074  SetBit(state.accel_status, ACCEL_ECEF_F);
1075 }
1078 /******************************************************************************
1079  * *
1080  * Transformation functions for the ANGULAR RATE representations *
1081  * *
1082  *****************************************************************************/
1087 {
1088  if (bit_is_set(state.rate_status, RATE_I)) {
1089  return;
1090  }
1091 
1092  if (bit_is_set(state.rate_status, RATE_F)) {
1094  }
1095  /* set bit to indicate this representation is computed */
1096  SetBit(state.rate_status, RATE_I);
1097 }
1098 
1100 {
1101  if (bit_is_set(state.rate_status, RATE_F)) {
1102  return;
1103  }
1104 
1105  if (bit_is_set(state.rate_status, RATE_I)) {
1107  }
1108  /* set bit to indicate this representation is computed */
1109  SetBit(state.rate_status, RATE_F);
1110 }
1111 
1115 /******************************************************************************
1116  * *
1117  * Transformation functions for the WIND- AND AIRSPEED representations *
1118  * *
1119  *****************************************************************************/
1124 {
1125  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1126  return;
1127  }
1128 
1129  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1132  }
1133  /* set bit to indicate this representation is computed */
1134  SetBit(state.wind_air_status , WINDSPEED_I);
1135 }
1136 
1138 {
1139  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1140  return;
1141  }
1142 
1143  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1145  }
1146  /* set bit to indicate this representation is computed */
1147  SetBit(state.wind_air_status, DOWNWIND_I);
1148 }
1149 
1151 {
1152  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1153  return;
1154  }
1155 
1156  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1158  }
1159  /* set bit to indicate this representation is computed */
1160  SetBit(state.wind_air_status, AIRSPEED_I);
1161 }
1162 
1164 {
1165  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1166  return;
1167  }
1168 
1169  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1172  }
1173  /* set bit to indicate this representation is computed */
1175 }
1176 
1178 {
1179  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1180  return;
1181  }
1182 
1183  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1185  }
1186  /* set bit to indicate this representation is computed */
1187  SetBit(state.wind_air_status, DOWNWIND_F);
1188 }
1189 
1191 {
1192  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1193  return;
1194  }
1195 
1196  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1198  }
1199  /* set bit to indicate this representation is computed */
1200  SetBit(state.wind_air_status, AIRSPEED_F);
1201 }
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:74
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:733
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
#define ACCEL_ECEF_I
Definition: state.h:102
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:709
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1137
#define ECEF_BFP_OF_REAL(_o, _i)
#define POS_LLA_I
Definition: state.h:70
#define RATE_F
Definition: state.h:113
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:953
int32_t y
North.
int32_t x
East.
#define AIRSPEED_F
Definition: state.h:125
#define WINDSPEED_I
Definition: state.h:120
#define INT32_SPEED_FRAC
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1123
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
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:1150
#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:150
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
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:220
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:92
void stateCalcSpeedEnu_f(void)
Definition: state.c:819
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
#define POS_UTM_F
Definition: state.h:76
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
#define SPEED_NED_I
Definition: state.h:86
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
#define POS_NED_F
Definition: state.h:73
void stateCalcPositionEcef_f(void)
Definition: state.c:352
#define SPEED_FLOAT_OF_BFP(_ai)
void stateCalcBodyRates_i(void)
Definition: state.c:1086
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
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:727
#define POS_NED_I
Definition: state.h:68
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:696
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:770
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:745
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:243
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
void stateCalcSpeedEnu_i(void)
Definition: state.c:621
#define FLOAT_VECT2_NORM(_v)
#define ACCEL_ECEF_F
Definition: state.h:104
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:703
#define SPEED_HNORM_I
Definition: state.h:88
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:892
#define SPEED_HDIR_I
Definition: state.h:89
#define AIRSPEED_I
Definition: state.h:122
void stateCalcPositionEnu_i(void)
Definition: state.c:178
#define SPEED_ECEF_I
Definition: state.h:85
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:263
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
#define ENU_FLOAT_OF_BFP(_o, _i)
uint32_t int32_sqrt(uint32_t in)
void stateCalcSpeedEcef_i(void)
Definition: state.c:672
void stateCalcPositionUtm_f(void)
Definition: state.c:316
void stateCalcPositionNed_f(void)
Definition: state.c:382
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:91
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:741
union State::@323 windspeed_f
Horizontal windspeed.
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
void stateCalcAccelEcef_f(void)
Definition: state.c:1047
uint8_t zone
UTM zone number.
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
void stateCalcAccelNed_f(void)
Definition: state.c:1015
#define VECT3_ENU_OF_NED(_o, _i)
void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:739
void stateCalcBodyRates_f(void)
Definition: state.c:1099
#define DOWNWIND_I
Definition: state.h:121
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
#define ACCEL_NED_F
Definition: state.h:105
#define POS_ECEF_I
Definition: state.h:67
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
void stateCalcAccelEcef_i(void)
Definition: state.c:985
#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:416
#define SPEED_ENU_I
Definition: state.h:87
union State::@322 windspeed_i
Horizontal windspeed in north/east/down.
#define WINDSPEED_F
Definition: state.h:123
void stateCalcSpeedEcef_f(void)
Definition: state.c:868
#define POS_LLA_F
Definition: state.h:75
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.
API to get/set the generic vehicle states.
Structure holding vehicle state data.
Definition: state.h:134
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
void stateCalcPositionEnu_f(void)
Definition: state.c:448
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
#define INT32_COURSE_NORMALIZE(_a)
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:917
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
#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:91
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
#define ENU_BFP_OF_REAL(_o, _i)
void stateCalcPositionNed_i(void)
Definition: state.c:99
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void stateCalcAirspeed_f(void)
Definition: state.c:1190
void stateCalcSpeedNed_i(void)
Definition: state.c:572
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
#define SPEED_BFP_OF_REAL(_af)
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:144
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
#define SPEED_HDIR_F
Definition: state.h:94
#define ECEF_FLOAT_OF_BFP(_o, _i)
void stateCalcPositionEcef_i(void)
Definition: state.c:68
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
void stateCalcPositionLla_f(void)
Definition: state.c:520
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
#define POS_ECEF_F
Definition: state.h:72
float y
in meters
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:73
#define SPEED_ECEF_F
Definition: state.h:90
#define RATE_I
Definition: state.h:112
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:85
#define ACCEL_NED_I
Definition: state.h:103
#define POS_ENU_I
Definition: state.h:69
#define LLA_FLOAT_OF_BFP(_o, _i)
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1177
#define SPEED_HNORM_F
Definition: state.h:93
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:1163
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:79
#define DOWNWIND_F
Definition: state.h:124
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.