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;
48  state.ned_to_body_orientation.status = 0;
49  state.rate_status = 0;
50  state.wind_air_status = 0;
51  state.ned_initialized_i = FALSE;
52  state.ned_initialized_f = FALSE;
53  state.utm_initialized_f = FALSE;
54 }
55 
56 
57 /*******************************************************************************
58  * *
59  * transformation functions for the POSITION representations *
60  * *
61  ******************************************************************************/
65 void stateCalcPositionEcef_i(void)
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)) {
72  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
73  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
74  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_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 */
77  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
78  SetBit(state.pos_status, POS_ECEF_F);
79  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
80  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
81  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_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 */
84  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
85  SetBit(state.pos_status, POS_ECEF_F);
86  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_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 
96 void stateCalcPositionNed_i(void)
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)) {
105  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
106  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
107  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
108  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
109  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
110  SetBit(state.pos_status, POS_ENU_I);
111  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
112  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
113  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_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 */
116  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
117  SetBit(state.pos_status, POS_NED_F);
118  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
121  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
122  SetBit(state.pos_status, POS_ECEF_F);
123  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
124  SetBit(state.pos_status, POS_NED_F);
125  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
126  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
127  ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_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)) {
133  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
134  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
135  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
136  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
137  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
138  SetBit(state.pos_status, POS_ENU_I);
139  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
140  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
141  /* transform utm_f -> ned_f -> ned_i, set status bits */
142  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
143  SetBit(state.pos_status, POS_NED_F);
144  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
147  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
148  SetBit(state.pos_status, POS_UTM_F);
149  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
150  SetBit(state.pos_status, POS_NED_F);
151  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
154  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
155  SetBit(state.pos_status, POS_LLA_F);
156  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
157  SetBit(state.pos_status, POS_UTM_F);
158  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
159  SetBit(state.pos_status, POS_NED_F);
160  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 
175 void stateCalcPositionEnu_i(void)
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)) {
184  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
185  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
186  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
187  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
188  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
189  SetBit(state.pos_status, POS_NED_I);
190  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
191  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
192  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_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 */
195  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
196  SetBit(state.pos_status, POS_ENU_F);
197  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
200  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
201  SetBit(state.pos_status, POS_ECEF_F);
202  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
203  SetBit(state.pos_status, POS_ENU_F);
204  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
205  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
206  enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_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)) {
212  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
213  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
214  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
215  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
216  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
217  SetBit(state.pos_status, POS_NED_I);
218  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
219  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
220  /* transform utm_f -> enu_f -> enu_i , set status bits */
221  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
222  SetBit(state.pos_status, POS_ENU_F);
223  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
226  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
227  SetBit(state.pos_status, POS_UTM_F);
228  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
229  SetBit(state.pos_status, POS_ENU_F);
230  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
233  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
234  SetBit(state.pos_status, POS_LLA_F);
235  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
236  SetBit(state.pos_status, POS_UTM_F);
237  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
238  SetBit(state.pos_status, POS_ENU_F);
239  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 
260 void stateCalcPositionLla_i(void)
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)) {
267  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
268  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
269  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
270  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
271  SetBit(state.pos_status, POS_ECEF_I);
272  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
275  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
276  SetBit(state.pos_status, POS_ECEF_I);
277  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
280  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
281  SetBit(state.pos_status, POS_ECEF_I);
282  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
285  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
286  SetBit(state.pos_status, POS_NED_I);
287  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
288  SetBit(state.pos_status, POS_ECEF_I);
289  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
292  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
293  SetBit(state.pos_status, POS_ENU_I);
294  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
295  SetBit(state.pos_status, POS_ECEF_I);
296  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
297  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
298  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
299  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
300  /* transform utm_f -> lla_f -> lla_i, set status bits */
301  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
302  SetBit(state.pos_status, POS_LLA_F);
303  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 
313 void stateCalcPositionUtm_f(void)
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)) {
320  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
321  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
322  /* transform lla_i -> lla_f -> utm_f, set status bits */
323  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
324  SetBit(state.pos_status, POS_LLA_F);
325  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
326  } else if (state.utm_initialized_f) {
327  if (bit_is_set(state.pos_status, POS_ENU_F)) {
328  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
329  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
330  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
331  SetBit(state.pos_status, POS_ENU_F);
332  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
333  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
334  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
335  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
336  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
337  SetBit(state.pos_status, POS_NED_F);
338  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_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 
349 void stateCalcPositionEcef_f(void)
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)) {
356  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
357  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
358  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_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 */
361  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
362  SetBit(state.pos_status, POS_ECEF_F);
363  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
364  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
365  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
366  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
367  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
368  SetBit(state.pos_status, POS_LLA_F);
369  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_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 
379 void stateCalcPositionNed_f(void)
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)) {
388  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
389  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
390  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
391  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
392  /* transform ecef_i -> ned_i -> ned_f, set status bits */
393  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
394  SetBit(state.pos_status, POS_NED_I);
395  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
396  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
397  ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_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);
402  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
403  SetBit(state.pos_status, POS_NED_I);
404  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_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)) {
410  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
411  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
412  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
413  SetBit(state.pos_status, POS_ENU_F);
414  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
415  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
416  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
417  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
418  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
419  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
420  /* transform lla_f -> utm_f -> ned, set status bits */
421  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
422  SetBit(state.pos_status, POS_UTM_F);
423  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_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 */
426  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
427  SetBit(state.pos_status, POS_LLA_F);
428  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
429  SetBit(state.pos_status, POS_UTM_F);
430  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_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 
445 void stateCalcPositionEnu_f(void)
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)) {
454  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
455  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
456  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
457  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
458  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
459  SetBit(state.pos_status, POS_NED_F);
460  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
461  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
462  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
463  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
464  /* transform ecef_i -> enu_i -> enu_f, set status bits */
465  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
466  SetBit(state.pos_status, POS_ENU_I);
467  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
468  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
469  enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_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);
474  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
475  SetBit(state.pos_status, POS_ENU_I);
476  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_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)) {
482  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
483  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
484  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
485  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
486  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
487  SetBit(state.pos_status, POS_NED_F);
488  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
489  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
490  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
491  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
492  /* transform lla_f -> utm_f -> enu, set status bits */
493  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
494  SetBit(state.pos_status, POS_UTM_F);
495  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_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 */
498  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
499  SetBit(state.pos_status, POS_LLA_F);
500  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
501  SetBit(state.pos_status, POS_UTM_F);
502  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_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 
517 void stateCalcPositionLla_f(void)
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)) {
524  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
525  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
526  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
527  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
528  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
529  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
530  SetBit(state.pos_status, POS_ECEF_F);
531  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_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 */
534  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
535  SetBit(state.pos_status, POS_ECEF_F);
536  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_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 */
539  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
540  SetBit(state.pos_status, POS_NED_F);
541  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
542  SetBit(state.pos_status, POS_ECEF_F);
543  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
544  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
545  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_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 
569 void stateCalcSpeedNed_i(void)
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)) {
578  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
579  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
580  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
581  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
582  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
583  SetBit(state.speed_status, SPEED_ENU_I);
584  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
585  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
586  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
587  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
588  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
589  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
590  SetBit(state.speed_status, SPEED_ECEF_I);
591  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
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)) {
597  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
598  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
599  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
600  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
601  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
602  SetBit(state.speed_status, SPEED_ENU_I);
603  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_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 
618 void stateCalcSpeedEnu_i(void)
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)) {
627  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
628  }
629  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
630  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
631  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
632  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
633  SetBit(state.pos_status, SPEED_NED_I);
634  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
635  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
636  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
637  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
638  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
639  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
640  SetBit(state.speed_status, SPEED_ECEF_I);
641  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
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)) {
647  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
648  }
649  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
650  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
651  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
652  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
653  SetBit(state.pos_status, SPEED_NED_I);
654  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_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 
669 void stateCalcSpeedEcef_i(void)
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)) {
676  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
677  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
678  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
679  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
680  /* transform ned_f -> ned_i -> ecef_i , set status bits */
681  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
682  SetBit(state.speed_status, SPEED_NED_I);
683  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_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 */
690  SetBit(state.speed_status, SPEED_ECEF_I);
691 }
692 
693 void stateCalcHorizontalSpeedNorm_i(void)
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)) {
700  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
701  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
702  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
703  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
704  INT32_SQRT(state.h_speed_norm_i, n2);
705  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
706  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
707  SetBit(state.speed_status, SPEED_HNORM_F);
708  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
709  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
710  uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x +
711  state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC;
712  INT32_SQRT(state.h_speed_norm_i, n2);
713  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
714  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
715  SetBit(state.speed_status, SPEED_HNORM_F);
716  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_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 */
719  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
720  SetBit(state.speed_status, SPEED_NED_I);
721  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
722  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
723  INT32_SQRT(state.h_speed_norm_i, n2);
724  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
725  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
726  SetBit(state.speed_status, SPEED_NED_F);
727  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
728  SetBit(state.speed_status, SPEED_HNORM_F);
729  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
730  } else {
731  //int32_t _norm_zero = 0;
732  //return _norm_zero;
733  }
734  /* set bit to indicate this representation is computed */
735  SetBit(state.speed_status, SPEED_HNORM_I);
736 }
737 
738 void stateCalcHorizontalSpeedDir_i(void)
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)) {
745  state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
746  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
747  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
748  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
749  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
750  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
751  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
752  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
753  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
754  SetBit(state.speed_status, SPEED_NED_I);
755  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
756  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
757  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
758  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
759  SetBit(state.speed_status, SPEED_ENU_I);
760  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
761  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
762  }
763  /* set bit to indicate this representation is computed */
764  SetBit(state.speed_status, SPEED_HDIR_I);
765 }
766 
767 void stateCalcSpeedNed_f(void)
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)) {
776  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
777  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
778  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
779  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
780  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
781  SetBit(state.speed_status, SPEED_ENU_F);
782  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
783  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
784  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
785  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
786  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
787  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
788  SetBit(state.speed_status, SPEED_ECEF_F);
789  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
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)) {
795  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
796  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
797  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
798  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
799  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
800  SetBit(state.speed_status, SPEED_ENU_F);
801  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_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 
816 void stateCalcSpeedEnu_f(void)
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)) {
825  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
826  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
827  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
828  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
829  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
830  SetBit(state.pos_status, SPEED_NED_F);
831  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
832  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
833  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
834  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
835  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
836  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
837  SetBit(state.speed_status, SPEED_ECEF_F);
838  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
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)) {
844  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
845  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
846  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
847  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
848  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
849  SetBit(state.pos_status, SPEED_NED_F);
850  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_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 
865 void stateCalcSpeedEcef_f(void)
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)) {
872  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
873  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
874  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
875  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
876  /* transform ned_f -> ned_i -> ecef_i , set status bits */
877  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
878  SetBit(state.speed_status, SPEED_NED_F);
879  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_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 */
886  SetBit(state.speed_status, SPEED_ECEF_F);
887 }
888 
889 void stateCalcHorizontalSpeedNorm_f(void)
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)) {
896  state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
897  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
898  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
899  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
900  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
901  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
902  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
903  SetBit(state.speed_status, SPEED_NED_F);
904  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
905  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
906  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
907  SetBit(state.speed_status, SPEED_ENU_F);
908  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
909  }
910  /* set bit to indicate this representation is computed */
911  SetBit(state.speed_status, SPEED_HNORM_F);
912 }
913 
914 void stateCalcHorizontalSpeedDir_f(void)
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)) {
921  state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
922  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
923  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
924  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
925  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
926  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
927  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
928  SetBit(state.speed_status, SPEED_NED_F);
929  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
930  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
931  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
932  SetBit(state.speed_status, SPEED_ENU_F);
933  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
934  }
935  /* set bit to indicate this representation is computed */
936  SetBit(state.speed_status, SPEED_HDIR_F);
937 }
942 /******************************************************************************
943  * *
944  * Transformation functions for the ACCELERATION representations *
945  * *
946  *****************************************************************************/
950 void stateCalcAccelNed_i(void)
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)) {
959  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
960  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
961  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
962  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
963  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
964  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
965  SetBit(state.accel_status, ACCEL_ECEF_I);
966  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
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 
981 void stateCalcAccelEcef_i(void)
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)) {
988  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
989  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
990  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
991  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
992  /* transform ned_f -> ned_i -> ecef_i , set status bits */
993  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
994  SetBit(state.accel_status, ACCEL_NED_I);
995  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_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 
1005 void stateCalcAccelNed_f(void)
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)) {
1014  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1015  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1016  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1017  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1018  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1019  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
1020  SetBit(state.accel_status, ACCEL_ECEF_F);
1021  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_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 
1036 void stateCalcAccelEcef_f(void)
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)) {
1043  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
1044  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1045  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1046  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1047  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1048  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1049  SetBit(state.accel_status, ACCEL_NED_F);
1050  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_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  *****************************************************************************/
1069 void stateCalcBodyRates_i(void)
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)) {
1076  RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
1077  }
1078  /* set bit to indicate this representation is computed */
1079  SetBit(state.rate_status, RATE_I);
1080 }
1081 
1082 void stateCalcBodyRates_f(void)
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)) {
1089  RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_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  *****************************************************************************/
1106 void stateCalcHorizontalWindspeed_i(void)
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)) {
1113  state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x);
1114  state.h_windspeed_i.y = SPEED_BFP_OF_REAL(state.h_windspeed_f.y);
1115  }
1116  /* set bit to indicate this representation is computed */
1117  SetBit(state.rate_status, WINDSPEED_I);
1118 }
1119 
1120 void stateCalcAirspeed_i(void)
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)) {
1127  state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f);
1128  }
1129  /* set bit to indicate this representation is computed */
1130  SetBit(state.wind_air_status, AIRSPEED_I);
1131 }
1132 
1133 void stateCalcHorizontalWindspeed_f(void)
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)) {
1140  state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x);
1141  state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.y);
1142  }
1143  /* set bit to indicate this representation is computed */
1144  SetBit(state.rate_status, WINDSPEED_F);
1145 }
1146 
1147 void stateCalcAirspeed_f(void)
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)) {
1154  state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i);
1155  }
1156  /* set bit to indicate this representation is computed */
1157  SetBit(state.wind_air_status, AIRSPEED_F);
1158 }
ecef_of_enu_pos_i
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.
Definition: pprz_geodetic_int.c:263
POS_ENU_F
#define POS_ENU_F
Definition: state.h:72
SPEEDS_BFP_OF_REAL
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:722
State::enu_speed_i
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:274
ACCEL_ECEF_I
#define ACCEL_ECEF_I
Definition: state.h:100
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:698
ECEF_BFP_OF_REAL
#define ECEF_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:150
POS_LLA_I
#define POS_LLA_I
Definition: state.h:68
RATE_F
#define RATE_F
Definition: state.h:111
enu_of_ecef_point_f
void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
Definition: pprz_geodetic_float.c:86
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
stateCalcAccelNed_i
void stateCalcAccelNed_i(void)
Definition: state.c:950
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
AIRSPEED_F
#define AIRSPEED_F
Definition: state.h:121
WINDSPEED_I
#define WINDSPEED_I
Definition: state.h:118
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
State::h_windspeed_f
struct FloatVect2 h_windspeed_f
Horizontal windspeed.
Definition: state.h:408
State::h_speed_dir_f
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:316
stateCalcHorizontalWindspeed_i
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1106
State::airspeed_f
float airspeed_f
Norm of relative air speed.
Definition: state.h:414
enu_of_ecef_pos_i
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.
Definition: pprz_geodetic_int.c:129
stateCalcAirspeed_i
void stateCalcAirspeed_i(void)
Definition: state.c:1120
NED_FLOAT_OF_BFP
#define NED_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:192
ned_of_ecef_point_f
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
Definition: pprz_geodetic_float.c:93
ned_of_ecef_vect_f
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
Definition: pprz_geodetic_float.c:106
State::ecef_pos_i
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:146
State::body_rates_f
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:379
enu_of_ecef_vect_f
void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef)
Definition: pprz_geodetic_float.c:101
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
ecef_of_ned_point_f
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
Definition: pprz_geodetic_float.c:158
SPEED_ENU_F
#define SPEED_ENU_F
Definition: state.h:90
stateCalcSpeedEnu_f
void stateCalcSpeedEnu_f(void)
Definition: state.c:816
State::lla_pos_i
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:153
FloatVect2::x
float x
Definition: pprz_algebra_float.h:49
POS_UTM_F
#define POS_UTM_F
Definition: state.h:74
State::lla_pos_f
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:201
State::ecef_pos_f
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:207
State::h_speed_norm_f
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:310
SPEED_NED_I
#define SPEED_NED_I
Definition: state.h:84
State::ecef_accel_i
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:339
State::ned_initialized_f
bool_t ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:219
State::ned_speed_i
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:268
State::rate_status
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:367
POS_NED_F
#define POS_NED_F
Definition: state.h:71
stateCalcPositionEcef_f
void stateCalcPositionEcef_f(void)
Definition: state.c:349
SPEED_FLOAT_OF_BFP
#define SPEED_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:219
State::h_windspeed_i
struct Int32Vect2 h_windspeed_i
Horizontal windspeed in north/east.
Definition: state.h:396
stateCalcBodyRates_i
void stateCalcBodyRates_i(void)
Definition: state.c:1069
State::h_speed_dir_i
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:287
enu_of_lla_point_f
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:113
SPEEDS_FLOAT_OF_BFP
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:716
POS_NED_I
#define POS_NED_I
Definition: state.h:66
stateCalcHorizontalSpeedNorm_i
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:693
ecef_of_ned_vect_f
void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
Definition: pprz_geodetic_float.c:193
NED_BFP_OF_REAL
#define NED_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:184
ned_of_ecef_vect_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.
Definition: pprz_geodetic_int.c:186
stateCalcSpeedNed_f
void stateCalcSpeedNed_f(void)
Definition: state.c:767
FALSE
#define FALSE
Definition: std.h:5
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:734
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
INT32_VECT3_NED_OF_ENU
#define INT32_VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:147
stateInit
void stateInit(void)
Definition: state.c:43
State::ned_pos_f
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:238
State::ned_accel_i
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:333
stateCalcSpeedEnu_i
void stateCalcSpeedEnu_i(void)
Definition: state.c:618
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:117
ACCEL_ECEF_F
#define ACCEL_ECEF_F
Definition: state.h:102
INT32_SQRT
#define INT32_SQRT(_out, _in)
Definition: pprz_algebra_int.h:229
State::speed_status
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:256
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:692
SPEED_HNORM_I
#define SPEED_HNORM_I
Definition: state.h:86
ned_of_ecef_pos_i
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.
Definition: pprz_geodetic_int.c:149
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
stateCalcHorizontalSpeedNorm_f
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:889
SPEED_HDIR_I
#define SPEED_HDIR_I
Definition: state.h:87
AIRSPEED_I
#define AIRSPEED_I
Definition: state.h:119
stateCalcPositionEnu_i
void stateCalcPositionEnu_i(void)
Definition: state.c:175
SPEED_ECEF_I
#define SPEED_ECEF_I
Definition: state.h:83
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
lla_of_utm_f
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
Definition: pprz_geodetic_float.c:330
uint32_t
unsigned long uint32_t
Definition: types.h:18
stateCalcPositionLla_i
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:260
State::accel_status
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:327
ENU_FLOAT_OF_BFP
#define ENU_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:198
stateCalcSpeedEcef_i
void stateCalcSpeedEcef_i(void)
Definition: state.c:669
stateCalcPositionUtm_f
void stateCalcPositionUtm_f(void)
Definition: state.c:313
stateCalcPositionNed_f
void stateCalcPositionNed_f(void)
Definition: state.c:379
ecef_of_lla_f
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
Definition: pprz_geodetic_float.c:242
UTM_OF_NED_ADD
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:92
ned_of_lla_point_f
void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:120
stateCalcHorizontalSpeedDir_i
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:738
State::enu_speed_f
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:304
stateCalcAccelEcef_f
void stateCalcAccelEcef_f(void)
Definition: state.c:1036
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
stateCalcAccelNed_f
void stateCalcAccelNed_f(void)
Definition: state.c:1005
VECT3_ENU_OF_NED
#define VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:140
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:728
stateCalcBodyRates_f
void stateCalcBodyRates_f(void)
Definition: state.c:1082
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
ACCEL_NED_F
#define ACCEL_NED_F
Definition: state.h:103
POS_ECEF_I
#define POS_ECEF_I
Definition: state.h:65
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
stateCalcAccelEcef_i
void stateCalcAccelEcef_i(void)
Definition: state.c:981
VECT3_NED_OF_ENU
#define VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:146
OrientationReps::status
uint8_t status
Holds the status bits for all orientation representations.
Definition: pprz_orientation_conversion.h:85
State::airspeed_i
int32_t airspeed_i
Norm of relative wind speed.
Definition: state.h:402
SPEED_ENU_I
#define SPEED_ENU_I
Definition: state.h:85
WINDSPEED_F
#define WINDSPEED_F
Definition: state.h:120
stateCalcSpeedEcef_f
void stateCalcSpeedEcef_f(void)
Definition: state.c:865
POS_LLA_F
#define POS_LLA_F
Definition: state.h:73
State::utm_initialized_f
bool_t utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:231
ecef_of_ned_pos_i
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.
Definition: pprz_geodetic_int.c:283
ned_of_lla_point_i
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.
Definition: pprz_geodetic_int.c:307
state.h
API to get/set the generic vehicle states.
State
Structure holding vehicle state data.
Definition: state.h:130
State::body_rates_i
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:373
stateCalcPositionEnu_f
void stateCalcPositionEnu_f(void)
Definition: state.c:445
State::wind_air_status
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:390
INT32_COURSE_NORMALIZE
#define INT32_COURSE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:131
stateCalcHorizontalSpeedDir_f
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:914
State::ned_accel_f
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:345
INT32_VECT3_ENU_OF_NED
#define INT32_VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:148
enu_of_ecef_vect_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.
Definition: pprz_geodetic_int.c:162
LLA_BFP_OF_REAL
#define LLA_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:167
SPEED_NED_F
#define SPEED_NED_F
Definition: state.h:89
State::utm_pos_f
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:188
State::h_speed_norm_i
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:280
State::ecef_speed_f
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:293
State::ecef_speed_i
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:262
ENU_BFP_OF_REAL
#define ENU_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:190
stateCalcPositionNed_i
void stateCalcPositionNed_i(void)
Definition: state.c:96
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Definition: pprz_geodetic_int.c:368
stateCalcAirspeed_f
void stateCalcAirspeed_f(void)
Definition: state.c:1147
stateCalcSpeedNed_i
void stateCalcSpeedNed_i(void)
Definition: state.c:569
State::ned_speed_f
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:298
State::enu_pos_i
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:181
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:218
State::pos_status
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:140
lla_of_ecef_i
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Definition: pprz_geodetic_int.c:335
SPEED_HDIR_F
#define SPEED_HDIR_F
Definition: state.h:92
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
State::ned_initialized_i
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
ECEF_FLOAT_OF_BFP
#define ECEF_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:156
enu_of_lla_point_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.
Definition: pprz_geodetic_int.c:295
stateCalcPositionEcef_i
void stateCalcPositionEcef_i(void)
Definition: state.c:65
State::enu_pos_f
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:245
FloatVect2::y
float y
Definition: pprz_algebra_float.h:50
stateCalcPositionLla_f
void stateCalcPositionLla_f(void)
Definition: state.c:517
State::ecef_accel_f
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:351
State::ned_to_body_orientation
struct OrientationReps ned_to_body_orientation
Definition: state.h:357
lla_of_ecef_f
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
Definition: pprz_geodetic_float.c:205
POS_ECEF_F
#define POS_ECEF_F
Definition: state.h:70
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
ENU_OF_UTM_DIFF
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:74
SPEED_ECEF_F
#define SPEED_ECEF_F
Definition: state.h:88
RATE_I
#define RATE_I
Definition: state.h:110
UTM_OF_ENU_ADD
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:86
ACCEL_NED_I
#define ACCEL_NED_I
Definition: state.h:101
POS_ENU_I
#define POS_ENU_I
Definition: state.h:67
LLA_FLOAT_OF_BFP
#define LLA_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:173
State::ned_pos_i
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:174
SPEED_HNORM_F
#define SPEED_HNORM_F
Definition: state.h:91
state
struct State state
Definition: state.c:36
utm_of_lla_f
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:304
int32_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:891
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
stateCalcHorizontalWindspeed_f
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1133
NED_OF_UTM_DIFF
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:80
ecef_of_ned_vect_i
void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned)
Rotate a vector from NED to ECEF.
Definition: pprz_geodetic_int.c:225