Paparazzi UAS  v5.15_devel-109-gee85905
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  return;
95  }
96  /* set bit to indicate this representation is computed */
97  SetBit(state.pos_status, POS_ECEF_I);
98 }
99 
101 {
102  if (bit_is_set(state.pos_status, POS_NED_I)) {
103  return;
104  }
105 
106  int errno = 0;
107  if (state.ned_initialized_i) {
108  if (bit_is_set(state.pos_status, POS_NED_F)) {
110  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
112  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
114  SetBit(state.pos_status, POS_ENU_I);
116  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
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  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
124  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
126  SetBit(state.pos_status, POS_ECEF_F);
128  SetBit(state.pos_status, POS_NED_F);
130  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
132  } else { /* could not get this representation, set errno */
133  errno = 1;
134  }
135  } else if (state.utm_initialized_f) {
136  if (bit_is_set(state.pos_status, POS_NED_F)) {
138  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
140  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
142  SetBit(state.pos_status, POS_ENU_I);
144  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
145  /* transform utm_f -> ned_f -> ned_i, set status bits */
147  SetBit(state.pos_status, POS_NED_F);
149  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
150  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
152  SetBit(state.pos_status, POS_UTM_F);
154  SetBit(state.pos_status, POS_NED_F);
156  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
157  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
159  SetBit(state.pos_status, POS_LLA_F);
161  SetBit(state.pos_status, POS_UTM_F);
163  SetBit(state.pos_status, POS_NED_F);
165  } else { /* could not get this representation, set errno */
166  errno = 2;
167  }
168  } else { /* ned coordinate system not initialized, set errno */
169  errno = 3;
170  }
171  if (errno) {
172  //struct NedCoor_i _ned_zero = {0};
173  //return _ned_zero;
174  return;
175  }
176  /* set bit to indicate this representation is computed */
177  SetBit(state.pos_status, POS_NED_I);
178 }
179 
181 {
182  if (bit_is_set(state.pos_status, POS_ENU_I)) {
183  return;
184  }
185 
186  int errno = 0;
187  if (state.ned_initialized_i) {
188  if (bit_is_set(state.pos_status, POS_NED_I)) {
190  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
192  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
194  SetBit(state.pos_status, POS_NED_I);
196  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
198  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
199  /* transform ecef_f -> enu_f, set status bit, then convert to int */
201  SetBit(state.pos_status, POS_ENU_F);
203  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
204  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
206  SetBit(state.pos_status, POS_ECEF_F);
208  SetBit(state.pos_status, POS_ENU_F);
210  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
212  } else { /* could not get this representation, set errno */
213  errno = 1;
214  }
215  } else if (state.utm_initialized_f) {
216  if (bit_is_set(state.pos_status, POS_ENU_F)) {
218  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
220  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
222  SetBit(state.pos_status, POS_NED_I);
224  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
225  /* transform utm_f -> enu_f -> enu_i , set status bits */
227  SetBit(state.pos_status, POS_ENU_F);
229  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
230  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
232  SetBit(state.pos_status, POS_UTM_F);
234  SetBit(state.pos_status, POS_ENU_F);
236  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
237  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
239  SetBit(state.pos_status, POS_LLA_F);
241  SetBit(state.pos_status, POS_UTM_F);
243  SetBit(state.pos_status, POS_ENU_F);
245  } else { /* could not get this representation, set errno */
246  errno = 2;
247  }
248  } else { /* ned coordinate system not initialized, set errno */
249  errno = 3;
250  }
251  if (errno) {
252  //struct EnuCoor_i _enu_zero = {0};
253  //return _enu_zero;
254  return;
255  }
256  /* set bit to indicate this representation is computed */
257  SetBit(state.pos_status, POS_ENU_I);
258 }
259 
268 {
269  if (bit_is_set(state.pos_status, POS_LLA_I)) {
270  return;
271  }
272 
273  int errno = 0;
274  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
276  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
277  /* transform ecef_f -> 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_UTM_F)) {
282  /* transform utm_f -> lla_f -> lla_i, set status bits */
284  SetBit(state.pos_status, POS_LLA_F);
286  } else if (state.ned_initialized_i) {
287  if (bit_is_set(state.pos_status, POS_NED_I)) {
288  /* transform ned_i -> ecef_i -> lla_i, set status bits */
290  SetBit(state.pos_status, POS_ECEF_I);
292  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
293  /* transform enu_i -> ecef_i -> lla_i, set status bits */
295  SetBit(state.pos_status, POS_ECEF_I);
297  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
298  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
300  SetBit(state.pos_status, POS_NED_I);
302  SetBit(state.pos_status, POS_ECEF_I);
304  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
305  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
307  SetBit(state.pos_status, POS_ENU_I);
309  SetBit(state.pos_status, POS_ECEF_I);
311  } else { /* could not get this representation, set errno */
312  errno = 1;
313  }
314  } else if (state.utm_initialized_f) {
315  if (bit_is_set(state.pos_status, POS_NED_I)) {
316  /* transform ned_i -> ned_f -> utm_f -> lla_f -> lla_i, set status bits */
318  SetBit(state.pos_status, POS_NED_F);
320  SetBit(state.pos_status, POS_UTM_F);
322  SetBit(state.pos_status, POS_LLA_F);
324  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
325  /* transform enu_i -> enu_f -> utm_f -> lla_f -> lla_i, set status bits */
327  SetBit(state.pos_status, POS_ENU_F);
329  SetBit(state.pos_status, POS_UTM_F);
331  SetBit(state.pos_status, POS_LLA_F);
333  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
334  /* transform ned_f -> utm_f -> lla_f -> lla_i, set status bits */
336  SetBit(state.pos_status, POS_UTM_F);
338  SetBit(state.pos_status, POS_LLA_F);
340  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
341  /* transform enu_f -> utm_f -> lla_f -> lla_i, set status bits */
343  SetBit(state.pos_status, POS_UTM_F);
345  SetBit(state.pos_status, POS_LLA_F);
347  } else { /* could not get this representation, set errno */
348  errno = 2;
349  }
350  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
352  } else { /* ned coordinate system not initialized, set errno */
353  errno = 3;
354  }
355  if (errno) {
356  /* could not get this representation, set errno */
357  //struct LlaCoor_i _lla_zero = {0};
358  //return _lla_zero;
359  return;
360  }
361  /* set bit to indicate this representation is computed */
362  SetBit(state.pos_status, POS_LLA_I);
363 }
364 
366 {
367  if (bit_is_set(state.pos_status, POS_UTM_F)) {
368  return;
369  }
370 
371  if (bit_is_set(state.pos_status, POS_LLA_F)) {
373  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
374  /* transform lla_i -> lla_f -> utm_f, set status bits */
376  SetBit(state.pos_status, POS_LLA_F);
378  } else if (state.utm_initialized_f) {
379  if (bit_is_set(state.pos_status, POS_ENU_F)) {
381  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
383  SetBit(state.pos_status, POS_ENU_F);
385  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
387  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
389  SetBit(state.pos_status, POS_NED_F);
391  }
392  } else {
393  /* could not get this representation, set errno */
394  //struct EcefCoor_f _ecef_zero = {0.0f};
395  //return _ecef_zero;
396  return;
397  }
398  /* set bit to indicate this representation is computed */
399  SetBit(state.pos_status, POS_UTM_F);
400 }
401 
403 {
404  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
405  return;
406  }
407 
408  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
410  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
412  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
413  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
415  SetBit(state.pos_status, POS_ECEF_F);
417  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
419  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
421  SetBit(state.pos_status, POS_LLA_F);
423  } else {
424  /* could not get this representation, set errno */
425  //struct EcefCoor_f _ecef_zero = {0.0f};
426  //return _ecef_zero;
427  return;
428  }
429  /* set bit to indicate this representation is computed */
430  SetBit(state.pos_status, POS_ECEF_F);
431 }
432 
434 {
435  if (bit_is_set(state.pos_status, POS_NED_F)) {
436  return;
437  }
438 
439  int errno = 0;
440  if (state.ned_initialized_f) {
441  if (bit_is_set(state.pos_status, POS_NED_I)) {
443  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
445  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
446  /* transform ecef_i -> ned_i -> ned_f, set status bits */
448  SetBit(state.pos_status, POS_NED_I);
450  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
452  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
453  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
454  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
455  SetBit(state.pos_status, POS_ECEF_I);
457  SetBit(state.pos_status, POS_NED_I);
459  } else { /* could not get this representation, set errno */
460  errno = 1;
461  }
462  } else if (state.utm_initialized_f) {
463  if (bit_is_set(state.pos_status, POS_NED_I)) {
465  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
467  SetBit(state.pos_status, POS_ENU_F);
469  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
471  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
473  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
474  /* transform lla_f -> utm_f -> ned, set status bits */
476  SetBit(state.pos_status, POS_UTM_F);
478  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
479  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
481  SetBit(state.pos_status, POS_LLA_F);
483  SetBit(state.pos_status, POS_UTM_F);
485  } else { /* could not get this representation, set errno */
486  errno = 2;
487  }
488  } else { /* ned coordinate system not initialized, set errno */
489  errno = 3;
490  }
491  if (errno) {
492  //struct NedCoor_f _ned_zero = {0.0f};
493  //return _ned_zero;
494  return;
495  }
496  /* set bit to indicate this representation is computed */
497  SetBit(state.pos_status, POS_NED_F);
498 }
499 
501 {
502  if (bit_is_set(state.pos_status, POS_ENU_F)) {
503  return;
504  }
505 
506  int errno = 0;
507  if (state.ned_initialized_f) {
508  if (bit_is_set(state.pos_status, POS_NED_F)) {
510  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
512  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
514  SetBit(state.pos_status, POS_NED_F);
516  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
518  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
519  /* transform ecef_i -> enu_i -> enu_f, set status bits */
521  SetBit(state.pos_status, POS_ENU_I);
523  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
525  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
526  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
527  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
528  SetBit(state.pos_status, POS_ECEF_I);
530  SetBit(state.pos_status, POS_ENU_I);
532  } else { /* could not get this representation, set errno */
533  errno = 1;
534  }
535  } else if (state.utm_initialized_f) {
536  if (bit_is_set(state.pos_status, POS_ENU_I)) {
538  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
540  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
542  SetBit(state.pos_status, POS_NED_F);
544  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
546  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
547  /* transform lla_f -> utm_f -> enu, set status bits */
549  SetBit(state.pos_status, POS_UTM_F);
551  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
552  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
554  SetBit(state.pos_status, POS_LLA_F);
556  SetBit(state.pos_status, POS_UTM_F);
558  } else { /* could not get this representation, set errno */
559  errno = 2;
560  }
561  } else { /* ned coordinate system not initialized, set errno */
562  errno = 3;
563  }
564  if (errno) {
565  //struct EnuCoor_f _enu_zero = {0.0f};
566  //return _enu_zero;
567  return;
568  }
569  /* set bit to indicate this representation is computed */
570  SetBit(state.pos_status, POS_ENU_F);
571 }
572 
574 {
575  if (bit_is_set(state.pos_status, POS_LLA_F)) {
576  return;
577  }
578 
579  int errno = 0;
580  if (bit_is_set(state.pos_status, POS_LLA_I)) {
582  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
584  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
585  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
587  SetBit(state.pos_status, POS_ECEF_F);
589  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
591  } else if (state.ned_initialized_f) {
592  if (bit_is_set(state.pos_status, POS_NED_F)) {
593  /* transform ned_f -> ecef_f -> lla_f, set status bits */
595  SetBit(state.pos_status, POS_ECEF_F);
597  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
598  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
600  SetBit(state.pos_status, POS_NED_F);
602  SetBit(state.pos_status, POS_ECEF_F);
604  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
605  /* transform enu_f -> ecef_f -> lla_f, set status bits */
607  SetBit(state.pos_status, POS_ECEF_F);
609  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
610  /* transform enu_i -> enu_f -> ecef_f -> lla_f, set status bits */
612  SetBit(state.pos_status, POS_ENU_F);
614  SetBit(state.pos_status, POS_ECEF_F);
616  } else { /* could not get this representation, set errno */
617  errno = 1;
618  }
619  } else if (state.utm_initialized_f) {
620  if (bit_is_set(state.pos_status, POS_NED_I)) {
621  /* transform ned_i -> ned_f -> utm_f -> lla_f, set status bits */
623  SetBit(state.pos_status, POS_NED_F);
625  SetBit(state.pos_status, POS_UTM_F);
627  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
628  /* transform enu_i -> enu_f -> utm_f -> lla_f, set status bits */
630  SetBit(state.pos_status, POS_ENU_F);
632  SetBit(state.pos_status, POS_UTM_F);
634  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
635  /* transform ned_f -> utm_f -> lla_f, set status bits */
637  SetBit(state.pos_status, POS_UTM_F);
639  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
640  /* transform enu_f -> utm_f -> lla_f, set status bits */
642  SetBit(state.pos_status, POS_UTM_F);
644  } else { /* could not get this representation, set errno */
645  errno = 2;
646  }
647  } else { /* ned coordinate system not initialized, set errno */
648  errno = 3;
649  }
650  if (errno) {
651  /* could not get this representation, set errno */
652  //struct LlaCoor_f _lla_zero = {0.0};
653  //return _lla_zero;
654  return;
655  }
656  /* set bit to indicate this representation is computed */
657  SetBit(state.pos_status, POS_LLA_F);
658 }
665 /******************************************************************************
666  * *
667  * Transformation functions for the SPEED representations *
668  * *
669  *****************************************************************************/
672 /************************ Set functions ****************************/
673 
675 {
676  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
677  return;
678  }
679 
680  int errno = 0;
681  if (state.ned_initialized_i) {
682  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
684  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
686  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
688  SetBit(state.speed_status, SPEED_ENU_I);
690  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
692  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
693  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
697  } else { /* could not get this representation, set errno */
698  errno = 1;
699  }
700  } else if (state.utm_initialized_f) {
701  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
703  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
705  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
707  SetBit(state.speed_status, SPEED_ENU_I);
709  } else { /* could not get this representation, set errno */
710  errno = 2;
711  }
712  } else { /* ned coordinate system not initialized, set errno */
713  errno = 3;
714  }
715  if (errno) {
716  //struct NedCoor_i _ned_zero = {0};
717  //return _ned_zero;
718  return;
719  }
720  /* set bit to indicate this representation is computed */
721  SetBit(state.speed_status, SPEED_NED_I);
722 }
723 
725 {
726  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
727  return;
728  }
729 
730  int errno = 0;
731  if (state.ned_initialized_i) {
732  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
734  }
735  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
737  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
739  SetBit(state.pos_status, SPEED_NED_I);
741  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
743  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
744  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
748  } else { /* could not get this representation, set errno */
749  errno = 1;
750  }
751  } else if (state.utm_initialized_f) {
752  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
754  }
755  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
757  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
759  SetBit(state.pos_status, SPEED_NED_I);
761  } else { /* could not get this representation, set errno */
762  errno = 2;
763  }
764  } else { /* ned coordinate system not initialized, set errno */
765  errno = 3;
766  }
767  if (errno) {
768  //struct EnuCoor_i _enu_zero = {0};
769  //return _enu_zero;
770  return;
771  }
772  /* set bit to indicate this representation is computed */
773  SetBit(state.speed_status, SPEED_ENU_I);
774 }
775 
777 {
778  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
779  return;
780  }
781 
782  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
784  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
786  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
787  /* transform ned_f -> ned_i -> ecef_i , set status bits */
789  SetBit(state.speed_status, SPEED_NED_I);
791  } else {
792  /* could not get this representation, set errno */
793  //struct EcefCoor_i _ecef_zero = {0};
794  //return _ecef_zero;
795  return;
796  }
797  /* set bit to indicate this representation is computed */
799 }
800 
802 {
803  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
804  return;
805  }
806 
807  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
809  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
813  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
817  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
821  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
825  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
826  /* transform ecef speed to ned, set status bit, then compute norm */
828  SetBit(state.speed_status, SPEED_NED_I);
832  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
834  SetBit(state.speed_status, SPEED_NED_F);
838  } else {
839  //int32_t _norm_zero = 0;
840  //return _norm_zero;
841  return;
842  }
843  /* set bit to indicate this representation is computed */
845 }
846 
848 {
849  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
850  return;
851  }
852 
853  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
855  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
858  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
861  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
863  SetBit(state.speed_status, SPEED_NED_I);
866  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
868  SetBit(state.speed_status, SPEED_ENU_I);
871  } else {
872  return;
873  }
874  /* set bit to indicate this representation is computed */
876 }
877 
879 {
880  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
881  return;
882  }
883 
884  int errno = 0;
885  if (state.ned_initialized_f) {
886  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
888  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
890  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
892  SetBit(state.speed_status, SPEED_ENU_F);
894  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
896  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
897  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
901  } else { /* could not get this representation, set errno */
902  errno = 1;
903  }
904  } else if (state.utm_initialized_f) {
905  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
907  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
909  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
911  SetBit(state.speed_status, SPEED_ENU_F);
913  } else { /* could not get this representation, set errno */
914  errno = 2;
915  }
916  } else { /* ned coordinate system not initialized, set errno */
917  errno = 3;
918  }
919  if (errno) {
920  //struct NedCoor_f _ned_zero = {0.0f};
921  //return _ned_zero;
922  return;
923  }
924  /* set bit to indicate this representation is computed */
925  SetBit(state.speed_status, SPEED_NED_F);
926 }
927 
929 {
930  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
931  return;
932  }
933 
934  int errno = 0;
935  if (state.ned_initialized_f) {
936  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
938  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
940  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
942  SetBit(state.pos_status, SPEED_NED_F);
944  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
946  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
947  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
951  } else { /* could not get this representation, set errno */
952  errno = 1;
953  }
954  } else if (state.utm_initialized_f) {
955  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
957  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
959  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
961  SetBit(state.pos_status, SPEED_NED_F);
963  } else { /* could not get this representation, set errno */
964  errno = 2;
965  }
966  } else { /* ned coordinate system not initialized, set errno */
967  errno = 3;
968  }
969  if (errno) {
970  //struct EnuCoor_f _enu_zero = {0};
971  //return _enu_zero;
972  return;
973  }
974  /* set bit to indicate this representation is computed */
975  SetBit(state.speed_status, SPEED_ENU_F);
976 }
977 
979 {
980  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
981  return;
982  }
983 
984  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
986  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
988  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
989  /* transform ned_f -> ned_i -> ecef_i , set status bits */
991  SetBit(state.speed_status, SPEED_NED_F);
993  } else {
994  /* could not get this representation, set errno */
995  //struct EcefCoor_f _ecef_zero = {0.0f};
996  //return _ecef_zero;
997  return;
998  }
999  /* set bit to indicate this representation is computed */
1000  SetBit(state.speed_status, SPEED_ECEF_F);
1001 }
1002 
1004 {
1005  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
1006  return;
1007  }
1008 
1009  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
1011  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1013  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1015  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1017  SetBit(state.speed_status, SPEED_NED_F);
1019  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1021  SetBit(state.speed_status, SPEED_ENU_F);
1023  } else {
1024  return;
1025  }
1026  /* set bit to indicate this representation is computed */
1027  SetBit(state.speed_status, SPEED_HNORM_F);
1028 }
1029 
1031 {
1032  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1033  return;
1034  }
1035 
1036  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1038  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1040  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1042  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1044  SetBit(state.speed_status, SPEED_NED_F);
1046  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1048  SetBit(state.speed_status, SPEED_ENU_F);
1050  } else {
1051  return;
1052  }
1053  /* set bit to indicate this representation is computed */
1054  SetBit(state.speed_status, SPEED_HDIR_F);
1055 }
1060 /******************************************************************************
1061  * *
1062  * Transformation functions for the ACCELERATION representations *
1063  * *
1064  *****************************************************************************/
1069 {
1070  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1071  return;
1072  }
1073 
1074  int errno = 0;
1075  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1077  }
1078  else if (state.ned_initialized_i) {
1079  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1081  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1082  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1084  SetBit(state.accel_status, ACCEL_ECEF_I);
1086  } else { /* could not get this representation, set errno */
1087  errno = 1;
1088  }
1089  } else { /* ned coordinate system not initialized, set errno */
1090  errno = 2;
1091  }
1092  if (errno) {
1093  //struct NedCoor_i _ned_zero = {0};
1094  //return _ned_zero;
1095  return;
1096  }
1097  /* set bit to indicate this representation is computed */
1098  SetBit(state.accel_status, ACCEL_NED_I);
1099 }
1100 
1102 {
1103  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1104  return;
1105  }
1106 
1107  int errno = 0;
1108  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1110  }
1111  else if (state.ned_initialized_i) {
1112  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1114  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1115  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1117  SetBit(state.accel_status, ACCEL_NED_I);
1119  } else {
1120  /* could not get this representation, set errno */
1121  errno = 1;
1122  }
1123  } else { /* ned coordinate system not initialized, set errno */
1124  errno = 2;
1125  }
1126  if (errno) {
1127  return;
1128  }
1129  /* set bit to indicate this representation is computed */
1130  SetBit(state.accel_status, ACCEL_ECEF_I);
1131 }
1132 
1134 {
1135  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1136  return;
1137  }
1138 
1139  int errno = 0;
1140  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1142  }
1143  else if (state.ned_initialized_f) {
1144  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1146  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1147  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1149  SetBit(state.accel_status, ACCEL_ECEF_F);
1151  } else { /* could not get this representation, set errno */
1152  errno = 1;
1153  }
1154  } else { /* ned coordinate system not initialized, set errno */
1155  errno = 2;
1156  }
1157  if (errno) {
1158  //struct NedCoor_f _ned_zero = {0.0f};
1159  //return _ned_zero;
1160  return;
1161  }
1162  /* set bit to indicate this representation is computed */
1163  SetBit(state.accel_status, ACCEL_NED_F);
1164 }
1165 
1167 {
1168  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1169  return;
1170  }
1171 
1172  int errno = 0;
1173  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1175  }
1176  else if (state.ned_initialized_f) {
1177  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1179  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1180  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1182  SetBit(state.accel_status, ACCEL_NED_F);
1184  } else {
1185  /* could not get this representation, set errno */
1186  errno = 1;
1187  }
1188  } else { /* ned coordinate system not initialized, set errno */
1189  errno = 2;
1190  }
1191  if (errno) {
1192  return;
1193  }
1194  /* set bit to indicate this representation is computed */
1195  SetBit(state.accel_status, ACCEL_ECEF_F);
1196 }
1199 /******************************************************************************
1200  * *
1201  * Transformation functions for the ANGULAR RATE representations *
1202  * *
1203  *****************************************************************************/
1208 {
1209  if (bit_is_set(state.rate_status, RATE_I)) {
1210  return;
1211  }
1212 
1213  if (bit_is_set(state.rate_status, RATE_F)) {
1215  } else {
1216  return;
1217  }
1218  /* set bit to indicate this representation is computed */
1219  SetBit(state.rate_status, RATE_I);
1220 }
1221 
1223 {
1224  if (bit_is_set(state.rate_status, RATE_F)) {
1225  return;
1226  }
1227 
1228  if (bit_is_set(state.rate_status, RATE_I)) {
1230  } else {
1231  return;
1232  }
1233  /* set bit to indicate this representation is computed */
1234  SetBit(state.rate_status, RATE_F);
1235 }
1236 
1240 /******************************************************************************
1241  * *
1242  * Transformation functions for the WIND- AND AIRSPEED representations *
1243  * *
1244  *****************************************************************************/
1249 {
1250  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1251  return;
1252  }
1253 
1254  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1257  } else {
1258  return;
1259  }
1260  /* set bit to indicate this representation is computed */
1261  SetBit(state.wind_air_status , WINDSPEED_I);
1262 }
1263 
1265 {
1266  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1267  return;
1268  }
1269 
1270  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1272  } else {
1273  return;
1274  }
1275  /* set bit to indicate this representation is computed */
1276  SetBit(state.wind_air_status, DOWNWIND_I);
1277 }
1278 
1280 {
1281  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1282  return;
1283  }
1284 
1285  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1287  } else {
1288  return;
1289  }
1290  /* set bit to indicate this representation is computed */
1291  SetBit(state.wind_air_status, AIRSPEED_I);
1292 }
1293 
1295 {
1296  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1297  return;
1298  }
1299 
1300  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1303  } else {
1304  return;
1305  }
1306  /* set bit to indicate this representation is computed */
1308 }
1309 
1311 {
1312  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1313  return;
1314  }
1315 
1316  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1318  } else {
1319  return;
1320  }
1321  /* set bit to indicate this representation is computed */
1322  SetBit(state.wind_air_status, DOWNWIND_F);
1323 }
1324 
1326 {
1327  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1328  return;
1329  }
1330 
1331  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1333  } else {
1334  return;
1335  }
1336  /* set bit to indicate this representation is computed */
1337  SetBit(state.wind_air_status, AIRSPEED_F);
1338 }
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:789
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:765
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1264
#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:1068
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:1248
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:1279
#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:928
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
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
#define POS_NED_F
Definition: state.h:73
void stateCalcPositionEcef_f(void)
Definition: state.c:402
#define SPEED_FLOAT_OF_BFP(_ai)
void stateCalcBodyRates_i(void)
Definition: state.c:1207
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:783
#define POS_NED_I
Definition: state.h:68
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:801
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:878
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
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:724
#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:759
#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:1003
#define SPEED_HDIR_I
Definition: state.h:89
#define AIRSPEED_I
Definition: state.h:122
void stateCalcPositionEnu_i(void)
Definition: state.c:180
#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:267
union State::@331 windspeed_i
Horizontal windspeed in north/east/down.
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:776
void stateCalcPositionUtm_f(void)
Definition: state.c:365
void stateCalcPositionNed_f(void)
Definition: state.c:433
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:96
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:847
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
void stateCalcAccelEcef_f(void)
Definition: state.c:1166
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:1133
#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:795
void stateCalcBodyRates_f(void)
Definition: state.c:1222
#define DOWNWIND_I
Definition: state.h:121
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
union State::@332 windspeed_f
Horizontal windspeed.
#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:1101
#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
#define WINDSPEED_F
Definition: state.h:123
void stateCalcSpeedEcef_f(void)
Definition: state.c:978
#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:500
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:1030
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:100
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:1325
void stateCalcSpeedNed_i(void)
Definition: state.c:674
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:573
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:78
#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:90
#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:1310
#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:1294
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:84
#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.