Paparazzi UAS  v5.15_devel-230-gc96ce27
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  /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
56  state.utm_origin_f.zone = 0;
57 }
58 
59 
60 /*******************************************************************************
61  * *
62  * transformation functions for the POSITION representations *
63  * *
64  ******************************************************************************/
68 void stateCalcPositionEcef_i(void)
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)) {
75  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
76  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
77  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_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 */
80  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
81  SetBit(state.pos_status, POS_ECEF_F);
82  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
83  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
84  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_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 */
87  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
88  SetBit(state.pos_status, POS_ECEF_F);
89  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_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 
100 void stateCalcPositionNed_i(void)
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)) {
109  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
110  } else if (bit_is_set(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_ENU_F)) {
113  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
114  SetBit(state.pos_status, POS_ENU_I);
115  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
116  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
117  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_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 */
120  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
121  SetBit(state.pos_status, POS_NED_F);
122  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
125  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
126  SetBit(state.pos_status, POS_ECEF_F);
127  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
128  SetBit(state.pos_status, POS_NED_F);
129  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
130  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
131  ned_of_lla_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_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)) {
137  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
138  } else if (bit_is_set(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_ENU_F)) {
141  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
142  SetBit(state.pos_status, POS_ENU_I);
143  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
144  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
145  /* transform utm_f -> ned_f -> ned_i, set status bits */
146  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
147  SetBit(state.pos_status, POS_NED_F);
148  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
151  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
152  SetBit(state.pos_status, POS_UTM_F);
153  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
154  SetBit(state.pos_status, POS_NED_F);
155  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 */
158  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
159  SetBit(state.pos_status, POS_LLA_F);
160  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
161  SetBit(state.pos_status, POS_UTM_F);
162  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
163  SetBit(state.pos_status, POS_NED_F);
164  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_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 
180 void stateCalcPositionEnu_i(void)
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)) {
189  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
190  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
191  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
192  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
193  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
194  SetBit(state.pos_status, POS_NED_I);
195  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
196  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
197  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_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 */
200  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
201  SetBit(state.pos_status, POS_ENU_F);
202  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
205  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
206  SetBit(state.pos_status, POS_ECEF_F);
207  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
208  SetBit(state.pos_status, POS_ENU_F);
209  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
210  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
211  enu_of_lla_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_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)) {
217  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
218  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
219  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
220  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
221  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
222  SetBit(state.pos_status, POS_NED_I);
223  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
224  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
225  /* transform utm_f -> enu_f -> enu_i , set status bits */
226  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
227  SetBit(state.pos_status, POS_ENU_F);
228  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
231  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
232  SetBit(state.pos_status, POS_UTM_F);
233  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
234  SetBit(state.pos_status, POS_ENU_F);
235  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 */
238  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
239  SetBit(state.pos_status, POS_LLA_F);
240  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
241  SetBit(state.pos_status, POS_UTM_F);
242  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
243  SetBit(state.pos_status, POS_ENU_F);
244  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_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 
267 void stateCalcPositionLla_i(void)
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)) {
275  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
276  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
277  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
278  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
279  SetBit(state.pos_status, POS_ECEF_I);
280  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
281  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
282  /* transform utm_f -> lla_f -> lla_i, set status bits */
283  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
284  SetBit(state.pos_status, POS_LLA_F);
285  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 */
289  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
290  SetBit(state.pos_status, POS_ECEF_I);
291  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
292  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
293  /* transform enu_i -> ecef_i -> lla_i, set status bits */
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_NED_F)) {
298  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
299  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
300  SetBit(state.pos_status, POS_NED_I);
301  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
302  SetBit(state.pos_status, POS_ECEF_I);
303  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
306  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
307  SetBit(state.pos_status, POS_ENU_I);
308  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
309  SetBit(state.pos_status, POS_ECEF_I);
310  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_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 */
317  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
318  SetBit(state.pos_status, POS_NED_F);
319  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
320  SetBit(state.pos_status, POS_UTM_F);
321  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
322  SetBit(state.pos_status, POS_LLA_F);
323  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 */
326  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
327  SetBit(state.pos_status, POS_ENU_F);
328  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
329  SetBit(state.pos_status, POS_UTM_F);
330  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
331  SetBit(state.pos_status, POS_LLA_F);
332  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 */
335  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
336  SetBit(state.pos_status, POS_UTM_F);
337  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
338  SetBit(state.pos_status, POS_LLA_F);
339  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 */
342  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
343  SetBit(state.pos_status, POS_UTM_F);
344  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
345  SetBit(state.pos_status, POS_LLA_F);
346  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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)) {
351  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_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 
365 void stateCalcPositionUtm_f(void)
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)) {
372  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
373  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
374  /* transform lla_i -> lla_f -> utm_f, set status bits */
375  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
376  SetBit(state.pos_status, POS_LLA_F);
377  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
378  } else if (state.utm_initialized_f) {
379  if (bit_is_set(state.pos_status, POS_ENU_F)) {
380  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
381  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
382  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
383  SetBit(state.pos_status, POS_ENU_F);
384  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
385  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
386  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
387  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
388  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
389  SetBit(state.pos_status, POS_NED_F);
390  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_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 
402 void stateCalcPositionEcef_f(void)
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)) {
409  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
410  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
411  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_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 */
414  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
415  SetBit(state.pos_status, POS_ECEF_F);
416  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
417  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
418  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
419  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
420  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
421  SetBit(state.pos_status, POS_LLA_F);
422  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_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 
433 void stateCalcPositionNed_f(void)
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)) {
442  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
443  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
444  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
445  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
446  /* transform ecef_i -> ned_i -> ned_f, set status bits */
447  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
448  SetBit(state.pos_status, POS_NED_I);
449  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
450  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
451  ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_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);
456  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
457  SetBit(state.pos_status, POS_NED_I);
458  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_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)) {
464  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
465  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
466  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
467  SetBit(state.pos_status, POS_ENU_F);
468  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
469  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
470  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
471  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
472  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
473  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
474  /* transform lla_f -> utm_f -> ned, set status bits */
475  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
476  SetBit(state.pos_status, POS_UTM_F);
477  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_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 */
480  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
481  SetBit(state.pos_status, POS_LLA_F);
482  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
483  SetBit(state.pos_status, POS_UTM_F);
484  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_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 
500 void stateCalcPositionEnu_f(void)
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)) {
509  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
510  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
511  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
512  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
513  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
514  SetBit(state.pos_status, POS_NED_F);
515  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
516  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
517  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
518  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
519  /* transform ecef_i -> enu_i -> enu_f, set status bits */
520  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
521  SetBit(state.pos_status, POS_ENU_I);
522  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
523  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
524  enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_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);
529  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
530  SetBit(state.pos_status, POS_ENU_I);
531  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_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)) {
537  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
538  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
539  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
540  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
541  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
542  SetBit(state.pos_status, POS_NED_F);
543  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
544  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
545  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
546  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
547  /* transform lla_f -> utm_f -> enu, set status bits */
548  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
549  SetBit(state.pos_status, POS_UTM_F);
550  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_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 */
553  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
554  SetBit(state.pos_status, POS_LLA_F);
555  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
556  SetBit(state.pos_status, POS_UTM_F);
557  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_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 
573 void stateCalcPositionLla_f(void)
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)) {
581  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
582  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
583  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
584  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
585  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
586  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
587  SetBit(state.pos_status, POS_ECEF_F);
588  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
589  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
590  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_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 */
594  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
595  SetBit(state.pos_status, POS_ECEF_F);
596  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_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 */
599  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
600  SetBit(state.pos_status, POS_NED_F);
601  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
602  SetBit(state.pos_status, POS_ECEF_F);
603  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
604  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
605  /* transform enu_f -> ecef_f -> lla_f, set status bits */
606  ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
607  SetBit(state.pos_status, POS_ECEF_F);
608  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_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 */
611  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
612  SetBit(state.pos_status, POS_ENU_F);
613  ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
614  SetBit(state.pos_status, POS_ECEF_F);
615  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_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 */
622  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
623  SetBit(state.pos_status, POS_NED_F);
624  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
625  SetBit(state.pos_status, POS_UTM_F);
626  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_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 */
629  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
630  SetBit(state.pos_status, POS_ENU_F);
631  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
632  SetBit(state.pos_status, POS_UTM_F);
633  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
634  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
635  /* transform ned_f -> utm_f -> lla_f, set status bits */
636  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
637  SetBit(state.pos_status, POS_UTM_F);
638  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
639  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
640  /* transform enu_f -> utm_f -> lla_f, set status bits */
641  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
642  SetBit(state.pos_status, POS_UTM_F);
643  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_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 
674 void stateCalcSpeedNed_i(void)
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)) {
683  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
684  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
685  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
686  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
687  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
688  SetBit(state.speed_status, SPEED_ENU_I);
689  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
690  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
691  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
692  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
693  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
694  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
695  SetBit(state.speed_status, SPEED_ECEF_I);
696  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
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)) {
702  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
703  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
704  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
705  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
706  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
707  SetBit(state.speed_status, SPEED_ENU_I);
708  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_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 
724 void stateCalcSpeedEnu_i(void)
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)) {
733  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
734  }
735  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
736  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
737  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
738  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
739  SetBit(state.pos_status, SPEED_NED_I);
740  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
741  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
742  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
743  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
744  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
745  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
746  SetBit(state.speed_status, SPEED_ECEF_I);
747  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
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)) {
753  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
754  }
755  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
756  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
757  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
758  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
759  SetBit(state.pos_status, SPEED_NED_I);
760  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_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 
776 void stateCalcSpeedEcef_i(void)
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)) {
783  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
784  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
785  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
786  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
787  /* transform ned_f -> ned_i -> ecef_i , set status bits */
788  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
789  SetBit(state.speed_status, SPEED_NED_I);
790  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_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 */
798  SetBit(state.speed_status, SPEED_ECEF_I);
799 }
800 
801 void stateCalcHorizontalSpeedNorm_i(void)
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)) {
808  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
809  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
810  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
811  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
812  state.h_speed_norm_i = int32_sqrt(n2);
813  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
814  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
815  SetBit(state.speed_status, SPEED_HNORM_F);
816  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
817  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
818  uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x +
819  state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC;
820  state.h_speed_norm_i = int32_sqrt(n2);
821  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
822  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
823  SetBit(state.speed_status, SPEED_HNORM_F);
824  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_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 */
827  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
828  SetBit(state.speed_status, SPEED_NED_I);
829  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
830  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
831  state.h_speed_norm_i = int32_sqrt(n2);
832  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
833  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
834  SetBit(state.speed_status, SPEED_NED_F);
835  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
836  SetBit(state.speed_status, SPEED_HNORM_F);
837  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_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 */
844  SetBit(state.speed_status, SPEED_HNORM_I);
845 }
846 
847 void stateCalcHorizontalSpeedDir_i(void)
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)) {
854  state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
855  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
856  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
857  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
858  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
859  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
860  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
861  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
862  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
863  SetBit(state.speed_status, SPEED_NED_I);
864  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
865  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
866  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
867  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
868  SetBit(state.speed_status, SPEED_ENU_I);
869  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
870  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
871  } else {
872  return;
873  }
874  /* set bit to indicate this representation is computed */
875  SetBit(state.speed_status, SPEED_HDIR_I);
876 }
877 
878 void stateCalcSpeedNed_f(void)
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)) {
887  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
888  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
889  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
890  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
891  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
892  SetBit(state.speed_status, SPEED_ENU_F);
893  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
894  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
895  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
896  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
897  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
898  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
899  SetBit(state.speed_status, SPEED_ECEF_F);
900  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
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)) {
906  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
907  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
908  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
909  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
910  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
911  SetBit(state.speed_status, SPEED_ENU_F);
912  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_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 
928 void stateCalcSpeedEnu_f(void)
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)) {
937  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
938  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
939  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
940  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
941  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
942  SetBit(state.pos_status, SPEED_NED_F);
943  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
944  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
945  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
946  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
947  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
948  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
949  SetBit(state.speed_status, SPEED_ECEF_F);
950  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
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)) {
956  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
957  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
958  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
959  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
960  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
961  SetBit(state.pos_status, SPEED_NED_F);
962  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_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 
978 void stateCalcSpeedEcef_f(void)
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)) {
985  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
986  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
987  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
988  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
989  /* transform ned_f -> ned_i -> ecef_i , set status bits */
990  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
991  SetBit(state.speed_status, SPEED_NED_F);
992  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_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 
1003 void stateCalcHorizontalSpeedNorm_f(void)
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)) {
1010  state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
1011  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1012  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1013  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1014  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
1015  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1016  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1017  SetBit(state.speed_status, SPEED_NED_F);
1018  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1019  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1020  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1021  SetBit(state.speed_status, SPEED_ENU_F);
1022  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_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 
1030 void stateCalcHorizontalSpeedDir_f(void)
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)) {
1037  state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
1038  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1039  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1040  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1041  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
1042  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1043  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1044  SetBit(state.speed_status, SPEED_NED_F);
1045  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1046  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1047  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1048  SetBit(state.speed_status, SPEED_ENU_F);
1049  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
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  *****************************************************************************/
1068 void stateCalcAccelNed_i(void)
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)) {
1076  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1077  }
1078  else if (state.ned_initialized_i) {
1079  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1080  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
1081  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1082  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1083  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1084  SetBit(state.accel_status, ACCEL_ECEF_I);
1085  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_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 
1101 void stateCalcAccelEcef_i(void)
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)) {
1109  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1110  }
1111  else if (state.ned_initialized_i) {
1112  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1113  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1114  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1115  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1116  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1117  SetBit(state.accel_status, ACCEL_NED_I);
1118  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_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 
1133 void stateCalcAccelNed_f(void)
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)) {
1141  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1142  }
1143  else if (state.ned_initialized_f) {
1144  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1145  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1146  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1147  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1148  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
1149  SetBit(state.accel_status, ACCEL_ECEF_F);
1150  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_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 
1166 void stateCalcAccelEcef_f(void)
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)) {
1174  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
1175  }
1176  else if (state.ned_initialized_f) {
1177  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1178  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1179  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1180  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1181  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1182  SetBit(state.accel_status, ACCEL_NED_F);
1183  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_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  *****************************************************************************/
1207 void stateCalcBodyRates_i(void)
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)) {
1214  RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
1215  } else {
1216  return;
1217  }
1218  /* set bit to indicate this representation is computed */
1219  SetBit(state.rate_status, RATE_I);
1220 }
1221 
1222 void stateCalcBodyRates_f(void)
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)) {
1229  RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_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  *****************************************************************************/
1248 void stateCalcHorizontalWindspeed_i(void)
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)) {
1255  state.windspeed_i.vect2.x = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.x);
1256  state.windspeed_i.vect2.y = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.y);
1257  } else {
1258  return;
1259  }
1260  /* set bit to indicate this representation is computed */
1261  SetBit(state.wind_air_status , WINDSPEED_I);
1262 }
1263 
1264 void stateCalcVerticalWindspeed_i(void)
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)) {
1271  state.windspeed_i.vect3.z = SPEED_BFP_OF_REAL(state.windspeed_f.vect3.z);
1272  } else {
1273  return;
1274  }
1275  /* set bit to indicate this representation is computed */
1276  SetBit(state.wind_air_status, DOWNWIND_I);
1277 }
1278 
1279 void stateCalcAirspeed_i(void)
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)) {
1286  state.airspeed_i = SPEED_BFP_OF_REAL(state.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 
1294 void stateCalcHorizontalWindspeed_f(void)
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)) {
1301  state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.x);
1302  state.windspeed_f.vect2.y = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.y);
1303  } else {
1304  return;
1305  }
1306  /* set bit to indicate this representation is computed */
1307  SetBit(state.wind_air_status, WINDSPEED_F);
1308 }
1309 
1310 void stateCalcVerticalWindspeed_f(void)
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)) {
1317  state.windspeed_f.vect3.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect3.z);
1318  } else {
1319  return;
1320  }
1321  /* set bit to indicate this representation is computed */
1322  SetBit(state.wind_air_status, DOWNWIND_F);
1323 }
1324 
1325 void stateCalcAirspeed_f(void)
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)) {
1332  state.airspeed_f = SPEED_FLOAT_OF_BFP(state.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 }
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:74
SPEEDS_BFP_OF_REAL
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:789
State::windspeed_i
union State::@338 windspeed_i
Horizontal windspeed in north/east/down.
State::enu_speed_i
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
ACCEL_ECEF_I
#define ACCEL_ECEF_I
Definition: state.h:102
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
State::utm_initialized_f
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
stateCalcVerticalWindspeed_i
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1264
ECEF_BFP_OF_REAL
#define ECEF_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:154
POS_LLA_I
#define POS_LLA_I
Definition: state.h:70
RATE_F
#define RATE_F
Definition: state.h:113
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:85
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
stateCalcAccelNed_i
void stateCalcAccelNed_i(void)
Definition: state.c:1068
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:125
WINDSPEED_I
#define WINDSPEED_I
Definition: state.h:120
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
State::h_speed_dir_f
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
stateCalcHorizontalWindspeed_i
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1248
State::airspeed_f
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
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:1279
NED_FLOAT_OF_BFP
#define NED_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:196
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:92
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:105
State::ecef_pos_i
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:150
State::body_rates_f
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
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:100
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
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:157
SPEED_ENU_F
#define SPEED_ENU_F
Definition: state.h:92
stateCalcSpeedEnu_f
void stateCalcSpeedEnu_f(void)
Definition: state.c:928
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
State::lla_pos_i
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
POS_UTM_F
#define POS_UTM_F
Definition: state.h:76
State::lla_pos_f
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
State::ecef_pos_f
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
State::h_speed_norm_f
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
SPEED_NED_I
#define SPEED_NED_I
Definition: state.h:86
State::ecef_accel_i
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
State::ned_speed_i
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
State::rate_status
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
ecef_of_enu_point_f
void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu)
Definition: pprz_geodetic_float.c:129
POS_NED_F
#define POS_NED_F
Definition: state.h:73
stateCalcPositionEcef_f
void stateCalcPositionEcef_f(void)
Definition: state.c:402
SPEED_FLOAT_OF_BFP
#define SPEED_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:219
stateCalcBodyRates_i
void stateCalcBodyRates_i(void)
Definition: state.c:1207
State::h_speed_dir_i
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
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:112
SPEEDS_FLOAT_OF_BFP
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:783
POS_NED_I
#define POS_NED_I
Definition: state.h:68
stateCalcHorizontalSpeedNorm_i
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:801
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:192
NED_BFP_OF_REAL
#define NED_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:188
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:878
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
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:151
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:243
State::ned_accel_i
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
stateCalcSpeedEnu_i
void stateCalcSpeedEnu_i(void)
Definition: state.c:724
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
ACCEL_ECEF_F
#define ACCEL_ECEF_F
Definition: state.h:104
State::speed_status
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
SPEED_HNORM_I
#define SPEED_HNORM_I
Definition: state.h:88
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:1003
SPEED_HDIR_I
#define SPEED_HDIR_I
Definition: state.h:89
AIRSPEED_I
#define AIRSPEED_I
Definition: state.h:122
stateCalcPositionEnu_i
void stateCalcPositionEnu_i(void)
Definition: state.c:180
SPEED_ECEF_I
#define SPEED_ECEF_I
Definition: state.h:85
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:344
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:267
State::accel_status
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
ENU_FLOAT_OF_BFP
#define ENU_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:202
int32_sqrt
uint32_t int32_sqrt(uint32_t in)
Definition: pprz_algebra_int.c:30
stateCalcSpeedEcef_i
void stateCalcSpeedEcef_i(void)
Definition: state.c:776
stateCalcPositionUtm_f
void stateCalcPositionUtm_f(void)
Definition: state.c:365
stateCalcPositionNed_f
void stateCalcPositionNed_f(void)
Definition: state.c:433
ecef_of_lla_f
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
Definition: pprz_geodetic_float.c:241
UTM_OF_NED_ADD
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:96
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:119
stateCalcHorizontalSpeedDir_i
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:847
State::enu_speed_f
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
stateCalcAccelEcef_f
void stateCalcAccelEcef_f(void)
Definition: state.c:1166
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
stateCalcAccelNed_f
void stateCalcAccelNed_f(void)
Definition: state.c:1133
VECT3_ENU_OF_NED
#define VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:144
ned_of_lla_pos_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.
Definition: pprz_geodetic_int.c:331
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
stateCalcBodyRates_f
void stateCalcBodyRates_f(void)
Definition: state.c:1222
DOWNWIND_I
#define DOWNWIND_I
Definition: state.h:121
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
ACCEL_NED_F
#define ACCEL_NED_F
Definition: state.h:105
POS_ECEF_I
#define POS_ECEF_I
Definition: state.h:67
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
stateCalcAccelEcef_i
void stateCalcAccelEcef_i(void)
Definition: state.c:1101
VECT3_NED_OF_ENU
#define VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:150
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:416
SPEED_ENU_I
#define SPEED_ENU_I
Definition: state.h:87
WINDSPEED_F
#define WINDSPEED_F
Definition: state.h:123
stateCalcSpeedEcef_f
void stateCalcSpeedEcef_f(void)
Definition: state.c:978
POS_LLA_F
#define POS_LLA_F
Definition: state.h:75
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
state.h
API to get/set the generic vehicle states.
State
Structure holding vehicle state data.
Definition: state.h:134
State::body_rates_i
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
stateCalcPositionEnu_f
void stateCalcPositionEnu_f(void)
Definition: state.c:500
State::wind_air_status
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
INT32_COURSE_NORMALIZE
#define INT32_COURSE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:131
stateCalcHorizontalSpeedDir_f
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1030
State::ned_accel_f
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
enu_of_lla_pos_i
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.
Definition: pprz_geodetic_int.c:319
INT32_VECT3_ENU_OF_NED
#define INT32_VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:152
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:171
SPEED_NED_F
#define SPEED_NED_F
Definition: state.h:91
State::utm_pos_f
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
State::h_speed_norm_i
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
State::ecef_speed_f
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
State::ecef_speed_i
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
ENU_BFP_OF_REAL
#define ENU_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:194
stateCalcPositionNed_i
void stateCalcPositionNed_i(void)
Definition: state.c:100
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
Definition: pprz_geodetic_int.c:392
stateCalcAirspeed_f
void stateCalcAirspeed_f(void)
Definition: state.c:1325
stateCalcSpeedNed_i
void stateCalcSpeedNed_i(void)
Definition: state.c:674
State::ned_speed_f
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
State::enu_pos_i
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
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:144
lla_of_ecef_i
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
Definition: pprz_geodetic_int.c:363
SPEED_HDIR_F
#define SPEED_HDIR_F
Definition: state.h:94
ECEF_FLOAT_OF_BFP
#define ECEF_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:160
stateCalcPositionEcef_i
void stateCalcPositionEcef_i(void)
Definition: state.c:68
State::enu_pos_f
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
stateCalcPositionLla_f
void stateCalcPositionLla_f(void)
Definition: state.c:573
State::ecef_accel_f
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
State::ned_to_body_orientation
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
lla_of_ecef_f
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
Definition: pprz_geodetic_float.c:204
POS_ECEF_F
#define POS_ECEF_F
Definition: state.h:72
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:78
SPEED_ECEF_F
#define SPEED_ECEF_F
Definition: state.h:90
RATE_I
#define RATE_I
Definition: state.h:112
UTM_OF_ENU_ADD
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:90
ACCEL_NED_I
#define ACCEL_NED_I
Definition: state.h:103
POS_ENU_I
#define POS_ENU_I
Definition: state.h:69
LLA_FLOAT_OF_BFP
#define LLA_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:177
State::ned_pos_i
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
stateCalcVerticalWindspeed_f
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1310
SPEED_HNORM_F
#define SPEED_HNORM_F
Definition: state.h:93
State::windspeed_f
union State::@339 windspeed_f
Horizontal windspeed.
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:308
int32_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:899
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
stateCalcHorizontalWindspeed_f
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1294
NED_OF_UTM_DIFF
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:84
DOWNWIND_F
#define DOWNWIND_F
Definition: state.h:124
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