Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
state.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011-2012 Felix Ruess <felix.ruess@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
34 #include "state.h"
35 
36 struct State state;
37 
43 void stateInit(void)
44 {
45  state.pos_status = 0;
46  state.speed_status = 0;
47  state.accel_status = 0;
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  }
95  /* set bit to indicate this representation is computed */
96  SetBit(state.pos_status, POS_ECEF_I);
97 }
98 
99 void stateCalcPositionNed_i(void)
100 {
101  if (bit_is_set(state.pos_status, POS_NED_I)) {
102  return;
103  }
104 
105  int errno = 0;
106  if (state.ned_initialized_i) {
107  if (bit_is_set(state.pos_status, POS_NED_F)) {
108  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
109  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
110  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
111  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
112  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
113  SetBit(state.pos_status, POS_ENU_I);
114  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
115  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
116  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
117  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
118  /* transform ecef_f -> ned_f, set status bit, then convert to int */
119  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
120  SetBit(state.pos_status, POS_NED_F);
121  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
122  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
123  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
124  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
125  SetBit(state.pos_status, POS_ECEF_F);
126  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
127  SetBit(state.pos_status, POS_NED_F);
128  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
129  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
130  ned_of_lla_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
131  } else { /* could not get this representation, set errno */
132  errno = 1;
133  }
134  } else if (state.utm_initialized_f) {
135  if (bit_is_set(state.pos_status, POS_NED_F)) {
136  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
137  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
138  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
139  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
140  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
141  SetBit(state.pos_status, POS_ENU_I);
142  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
143  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
144  /* transform utm_f -> ned_f -> ned_i, set status bits */
145  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
146  SetBit(state.pos_status, POS_NED_F);
147  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
148  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
149  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
150  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
151  SetBit(state.pos_status, POS_UTM_F);
152  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
153  SetBit(state.pos_status, POS_NED_F);
154  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
155  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
156  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
157  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
158  SetBit(state.pos_status, POS_LLA_F);
159  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
160  SetBit(state.pos_status, POS_UTM_F);
161  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
162  SetBit(state.pos_status, POS_NED_F);
163  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
164  } else { /* could not get this representation, set errno */
165  errno = 2;
166  }
167  } else { /* ned coordinate system not initialized, set errno */
168  errno = 3;
169  }
170  if (errno) {
171  //struct NedCoor_i _ned_zero = {0};
172  //return _ned_zero;
173  }
174  /* set bit to indicate this representation is computed */
175  SetBit(state.pos_status, POS_NED_I);
176 }
177 
178 void stateCalcPositionEnu_i(void)
179 {
180  if (bit_is_set(state.pos_status, POS_ENU_I)) {
181  return;
182  }
183 
184  int errno = 0;
185  if (state.ned_initialized_i) {
186  if (bit_is_set(state.pos_status, POS_NED_I)) {
187  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
188  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
189  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
190  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
191  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
192  SetBit(state.pos_status, POS_NED_I);
193  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
194  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
195  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
196  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
197  /* transform ecef_f -> enu_f, set status bit, then convert to int */
198  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
199  SetBit(state.pos_status, POS_ENU_F);
200  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
201  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
202  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
203  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
204  SetBit(state.pos_status, POS_ECEF_F);
205  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
206  SetBit(state.pos_status, POS_ENU_F);
207  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
208  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
209  enu_of_lla_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
210  } else { /* could not get this representation, set errno */
211  errno = 1;
212  }
213  } else if (state.utm_initialized_f) {
214  if (bit_is_set(state.pos_status, POS_ENU_F)) {
215  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
216  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
217  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
218  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
219  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
220  SetBit(state.pos_status, POS_NED_I);
221  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
222  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
223  /* transform utm_f -> enu_f -> enu_i , set status bits */
224  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
225  SetBit(state.pos_status, POS_ENU_F);
226  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
227  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
228  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
229  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
230  SetBit(state.pos_status, POS_UTM_F);
231  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
232  SetBit(state.pos_status, POS_ENU_F);
233  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
234  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
235  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
236  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
237  SetBit(state.pos_status, POS_LLA_F);
238  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
239  SetBit(state.pos_status, POS_UTM_F);
240  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
241  SetBit(state.pos_status, POS_ENU_F);
242  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
243  } else { /* could not get this representation, set errno */
244  errno = 2;
245  }
246  } else { /* ned coordinate system not initialized, set errno */
247  errno = 3;
248  }
249  if (errno) {
250  //struct EnuCoor_i _enu_zero = {0};
251  //return _enu_zero;
252  }
253  /* set bit to indicate this representation is computed */
254  SetBit(state.pos_status, POS_ENU_I);
255 }
256 
263 void stateCalcPositionLla_i(void)
264 {
265  if (bit_is_set(state.pos_status, POS_LLA_I)) {
266  return;
267  }
268 
269  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
270  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
271  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
272  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
273  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
274  SetBit(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_NED_I) && state.ned_initialized_i) {
277  /* transform ned_i -> ecef_i -> lla_i, set status bits */
278  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
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_ENU_I) && state.ned_initialized_i) {
282  /* transform enu_i -> ecef_i -> lla_i, set status bits */
283  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
284  SetBit(state.pos_status, POS_ECEF_I);
285  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
286  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
287  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
288  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
289  SetBit(state.pos_status, POS_NED_I);
290  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
291  SetBit(state.pos_status, POS_ECEF_I);
292  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
293  } else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
294  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
295  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
296  SetBit(state.pos_status, POS_ENU_I);
297  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
298  SetBit(state.pos_status, POS_ECEF_I);
299  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
300  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
301  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
302  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
303  /* transform utm_f -> lla_f -> lla_i, set status bits */
304  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
305  SetBit(state.pos_status, POS_LLA_F);
306  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
307  } else {
308  /* could not get this representation, set errno */
309  //struct LlaCoor_i _lla_zero = {0};
310  //return _lla_zero;
311  }
312  /* set bit to indicate this representation is computed */
313  SetBit(state.pos_status, POS_LLA_I);
314 }
315 
316 void stateCalcPositionUtm_f(void)
317 {
318  if (bit_is_set(state.pos_status, POS_UTM_F)) {
319  return;
320  }
321 
322  if (bit_is_set(state.pos_status, POS_LLA_F)) {
323  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
324  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
325  /* transform lla_i -> lla_f -> utm_f, set status bits */
326  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
327  SetBit(state.pos_status, POS_LLA_F);
328  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
329  } else if (state.utm_initialized_f) {
330  if (bit_is_set(state.pos_status, POS_ENU_F)) {
331  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
332  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
333  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
334  SetBit(state.pos_status, POS_ENU_F);
335  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
336  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
337  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
338  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
339  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
340  SetBit(state.pos_status, POS_NED_F);
341  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
342  }
343  } else {
344  /* could not get this representation, set errno */
345  //struct EcefCoor_f _ecef_zero = {0.0f};
346  //return _ecef_zero;
347  }
348  /* set bit to indicate this representation is computed */
349  SetBit(state.pos_status, POS_UTM_F);
350 }
351 
352 void stateCalcPositionEcef_f(void)
353 {
354  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
355  return;
356  }
357 
358  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
359  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
360  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
361  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
362  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
363  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
364  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
365  SetBit(state.pos_status, POS_ECEF_F);
366  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
367  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
368  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
369  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
370  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
371  SetBit(state.pos_status, POS_LLA_F);
372  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
373  } else {
374  /* could not get this representation, set errno */
375  //struct EcefCoor_f _ecef_zero = {0.0f};
376  //return _ecef_zero;
377  }
378  /* set bit to indicate this representation is computed */
379  SetBit(state.pos_status, POS_ECEF_F);
380 }
381 
382 void stateCalcPositionNed_f(void)
383 {
384  if (bit_is_set(state.pos_status, POS_NED_F)) {
385  return;
386  }
387 
388  int errno = 0;
389  if (state.ned_initialized_f) {
390  if (bit_is_set(state.pos_status, POS_NED_I)) {
391  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
392  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
393  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
394  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
395  /* transform ecef_i -> ned_i -> ned_f, set status bits */
396  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
397  SetBit(state.pos_status, POS_NED_I);
398  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
399  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
400  ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f);
401  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
402  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
403  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
404  SetBit(state.pos_status, POS_ECEF_I);
405  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
406  SetBit(state.pos_status, POS_NED_I);
407  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
408  } else { /* could not get this representation, set errno */
409  errno = 1;
410  }
411  } else if (state.utm_initialized_f) {
412  if (bit_is_set(state.pos_status, POS_NED_I)) {
413  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
414  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
415  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
416  SetBit(state.pos_status, POS_ENU_F);
417  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
418  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
419  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
420  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
421  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
422  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
423  /* transform lla_f -> utm_f -> ned, set status bits */
424  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
425  SetBit(state.pos_status, POS_UTM_F);
426  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
427  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
428  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
429  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
430  SetBit(state.pos_status, POS_LLA_F);
431  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
432  SetBit(state.pos_status, POS_UTM_F);
433  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
434  } else { /* could not get this representation, set errno */
435  errno = 2;
436  }
437  } else { /* ned coordinate system not initialized, set errno */
438  errno = 3;
439  }
440  if (errno) {
441  //struct NedCoor_f _ned_zero = {0.0f};
442  //return _ned_zero;
443  }
444  /* set bit to indicate this representation is computed */
445  SetBit(state.pos_status, POS_NED_F);
446 }
447 
448 void stateCalcPositionEnu_f(void)
449 {
450  if (bit_is_set(state.pos_status, POS_ENU_F)) {
451  return;
452  }
453 
454  int errno = 0;
455  if (state.ned_initialized_f) {
456  if (bit_is_set(state.pos_status, POS_NED_F)) {
457  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
458  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
459  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
460  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
461  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
462  SetBit(state.pos_status, POS_NED_F);
463  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
464  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
465  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
466  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
467  /* transform ecef_i -> enu_i -> enu_f, set status bits */
468  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
469  SetBit(state.pos_status, POS_ENU_I);
470  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
471  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
472  enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
473  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
474  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
475  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
476  SetBit(state.pos_status, POS_ECEF_I);
477  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
478  SetBit(state.pos_status, POS_ENU_I);
479  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
480  } else { /* could not get this representation, set errno */
481  errno = 1;
482  }
483  } else if (state.utm_initialized_f) {
484  if (bit_is_set(state.pos_status, POS_ENU_I)) {
485  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
486  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
487  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
488  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
489  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
490  SetBit(state.pos_status, POS_NED_F);
491  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
492  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
493  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
494  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
495  /* transform lla_f -> utm_f -> enu, set status bits */
496  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
497  SetBit(state.pos_status, POS_UTM_F);
498  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
499  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
500  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
501  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
502  SetBit(state.pos_status, POS_LLA_F);
503  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
504  SetBit(state.pos_status, POS_UTM_F);
505  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
506  } else { /* could not get this representation, set errno */
507  errno = 2;
508  }
509  } else { /* ned coordinate system not initialized, set errno */
510  errno = 3;
511  }
512  if (errno) {
513  //struct EnuCoor_f _enu_zero = {0.0f};
514  //return _enu_zero;
515  }
516  /* set bit to indicate this representation is computed */
517  SetBit(state.pos_status, POS_ENU_F);
518 }
519 
520 void stateCalcPositionLla_f(void)
521 {
522  if (bit_is_set(state.pos_status, POS_LLA_F)) {
523  return;
524  }
525 
526  if (bit_is_set(state.pos_status, POS_LLA_I)) {
527  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
528  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
529  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
530  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
531  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
532  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
533  SetBit(state.pos_status, POS_ECEF_F);
534  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
535  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
536  /* transform ned_f -> ecef_f -> lla_f, set status bits */
537  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
538  SetBit(state.pos_status, POS_ECEF_F);
539  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
540  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_f) {
541  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
542  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
543  SetBit(state.pos_status, POS_NED_F);
544  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
545  SetBit(state.pos_status, POS_ECEF_F);
546  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
547  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
548  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
549  } else {
550  /* could not get this representation, set errno */
551  //struct LlaCoor_f _lla_zero = {0.0};
552  //return _lla_zero;
553  }
554  /* set bit to indicate this representation is computed */
555  SetBit(state.pos_status, POS_LLA_F);
556 }
563 /******************************************************************************
564  * *
565  * Transformation functions for the SPEED representations *
566  * *
567  *****************************************************************************/
570 /************************ Set functions ****************************/
571 
572 void stateCalcSpeedNed_i(void)
573 {
574  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
575  return;
576  }
577 
578  int errno = 0;
579  if (state.ned_initialized_i) {
580  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
581  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
582  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
583  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
584  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
585  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
586  SetBit(state.speed_status, SPEED_ENU_I);
587  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
588  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
589  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
590  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
591  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
592  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
593  SetBit(state.speed_status, SPEED_ECEF_I);
594  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
595  } else { /* could not get this representation, set errno */
596  errno = 1;
597  }
598  } else if (state.utm_initialized_f) {
599  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
600  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
601  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
602  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
603  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
604  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
605  SetBit(state.speed_status, SPEED_ENU_I);
606  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
607  } else { /* could not get this representation, set errno */
608  errno = 2;
609  }
610  } else { /* ned coordinate system not initialized, set errno */
611  errno = 3;
612  }
613  if (errno) {
614  //struct NedCoor_i _ned_zero = {0};
615  //return _ned_zero;
616  }
617  /* set bit to indicate this representation is computed */
618  SetBit(state.speed_status, SPEED_NED_I);
619 }
620 
621 void stateCalcSpeedEnu_i(void)
622 {
623  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
624  return;
625  }
626 
627  int errno = 0;
628  if (state.ned_initialized_i) {
629  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
630  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
631  }
632  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
633  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
634  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
635  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
636  SetBit(state.pos_status, SPEED_NED_I);
637  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
638  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
639  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
640  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
641  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
642  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
643  SetBit(state.speed_status, SPEED_ECEF_I);
644  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
645  } else { /* could not get this representation, set errno */
646  errno = 1;
647  }
648  } else if (state.utm_initialized_f) {
649  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
650  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
651  }
652  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
653  ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
654  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
655  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
656  SetBit(state.pos_status, SPEED_NED_I);
657  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
658  } else { /* could not get this representation, set errno */
659  errno = 2;
660  }
661  } else { /* ned coordinate system not initialized, set errno */
662  errno = 3;
663  }
664  if (errno) {
665  //struct EnuCoor_i _enu_zero = {0};
666  //return _enu_zero;
667  }
668  /* set bit to indicate this representation is computed */
669  SetBit(state.speed_status, SPEED_ENU_I);
670 }
671 
672 void stateCalcSpeedEcef_i(void)
673 {
674  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
675  return;
676  }
677 
678  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
679  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
680  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
681  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
682  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
683  /* transform ned_f -> ned_i -> ecef_i , set status bits */
684  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
685  SetBit(state.speed_status, SPEED_NED_I);
686  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
687  } else {
688  /* could not get this representation, set errno */
689  //struct EcefCoor_i _ecef_zero = {0};
690  //return _ecef_zero;
691  }
692  /* set bit to indicate this representation is computed */
693  SetBit(state.speed_status, SPEED_ECEF_I);
694 }
695 
696 void stateCalcHorizontalSpeedNorm_i(void)
697 {
698  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
699  return;
700  }
701 
702  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
703  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
704  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
705  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
706  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
707  state.h_speed_norm_i = int32_sqrt(n2);
708  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
709  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
710  SetBit(state.speed_status, SPEED_HNORM_F);
711  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
712  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
713  uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x +
714  state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC;
715  state.h_speed_norm_i = int32_sqrt(n2);
716  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
717  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
718  SetBit(state.speed_status, SPEED_HNORM_F);
719  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
720  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
721  /* transform ecef speed to ned, set status bit, then compute norm */
722  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
723  SetBit(state.speed_status, SPEED_NED_I);
724  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
725  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
726  state.h_speed_norm_i = int32_sqrt(n2);
727  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
728  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
729  SetBit(state.speed_status, SPEED_NED_F);
730  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
731  SetBit(state.speed_status, SPEED_HNORM_F);
732  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
733  } else {
734  //int32_t _norm_zero = 0;
735  //return _norm_zero;
736  }
737  /* set bit to indicate this representation is computed */
738  SetBit(state.speed_status, SPEED_HNORM_I);
739 }
740 
741 void stateCalcHorizontalSpeedDir_i(void)
742 {
743  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
744  return;
745  }
746 
747  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
748  state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
749  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
750  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
751  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
752  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
753  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
754  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
755  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
756  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
757  SetBit(state.speed_status, SPEED_NED_I);
758  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
759  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
760  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
761  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
762  SetBit(state.speed_status, SPEED_ENU_I);
763  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
764  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
765  }
766  /* set bit to indicate this representation is computed */
767  SetBit(state.speed_status, SPEED_HDIR_I);
768 }
769 
770 void stateCalcSpeedNed_f(void)
771 {
772  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
773  return;
774  }
775 
776  int errno = 0;
777  if (state.ned_initialized_f) {
778  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
779  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
780  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
781  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
782  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
783  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
784  SetBit(state.speed_status, SPEED_ENU_F);
785  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
786  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
787  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
788  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
789  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
790  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
791  SetBit(state.speed_status, SPEED_ECEF_F);
792  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
793  } else { /* could not get this representation, set errno */
794  errno = 1;
795  }
796  } else if (state.utm_initialized_f) {
797  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
798  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
799  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
800  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
801  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
802  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
803  SetBit(state.speed_status, SPEED_ENU_F);
804  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
805  } else { /* could not get this representation, set errno */
806  errno = 2;
807  }
808  } else { /* ned coordinate system not initialized, set errno */
809  errno = 3;
810  }
811  if (errno) {
812  //struct NedCoor_f _ned_zero = {0.0f};
813  //return _ned_zero;
814  }
815  /* set bit to indicate this representation is computed */
816  SetBit(state.speed_status, SPEED_NED_F);
817 }
818 
819 void stateCalcSpeedEnu_f(void)
820 {
821  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
822  return;
823  }
824 
825  int errno = 0;
826  if (state.ned_initialized_f) {
827  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
828  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
829  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
830  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
831  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
832  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
833  SetBit(state.pos_status, SPEED_NED_F);
834  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
835  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
836  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
837  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
838  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
839  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
840  SetBit(state.speed_status, SPEED_ECEF_F);
841  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
842  } else { /* could not get this representation, set errno */
843  errno = 1;
844  }
845  } else if (state.utm_initialized_f) {
846  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
847  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
848  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
849  ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
850  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
851  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
852  SetBit(state.pos_status, SPEED_NED_F);
853  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
854  } else { /* could not get this representation, set errno */
855  errno = 2;
856  }
857  } else { /* ned coordinate system not initialized, set errno */
858  errno = 3;
859  }
860  if (errno) {
861  //struct EnuCoor_f _enu_zero = {0};
862  //return _enu_zero;
863  }
864  /* set bit to indicate this representation is computed */
865  SetBit(state.speed_status, SPEED_ENU_F);
866 }
867 
868 void stateCalcSpeedEcef_f(void)
869 {
870  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
871  return;
872  }
873 
874  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
875  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
876  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
877  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
878  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
879  /* transform ned_f -> ned_i -> ecef_i , set status bits */
880  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
881  SetBit(state.speed_status, SPEED_NED_F);
882  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
883  } else {
884  /* could not get this representation, set errno */
885  //struct EcefCoor_f _ecef_zero = {0.0f};
886  //return _ecef_zero;
887  }
888  /* set bit to indicate this representation is computed */
889  SetBit(state.speed_status, SPEED_ECEF_F);
890 }
891 
892 void stateCalcHorizontalSpeedNorm_f(void)
893 {
894  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
895  return;
896  }
897 
898  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
899  state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
900  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
901  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
902  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
903  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
904  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
905  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
906  SetBit(state.speed_status, SPEED_NED_F);
907  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
908  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
909  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
910  SetBit(state.speed_status, SPEED_ENU_F);
911  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
912  }
913  /* set bit to indicate this representation is computed */
914  SetBit(state.speed_status, SPEED_HNORM_F);
915 }
916 
917 void stateCalcHorizontalSpeedDir_f(void)
918 {
919  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
920  return;
921  }
922 
923  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
924  state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
925  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
926  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
927  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
928  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
929  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
930  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
931  SetBit(state.speed_status, SPEED_NED_F);
932  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
933  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
934  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
935  SetBit(state.speed_status, SPEED_ENU_F);
936  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
937  }
938  /* set bit to indicate this representation is computed */
939  SetBit(state.speed_status, SPEED_HDIR_F);
940 }
945 /******************************************************************************
946  * *
947  * Transformation functions for the ACCELERATION representations *
948  * *
949  *****************************************************************************/
953 void stateCalcAccelNed_i(void)
954 {
955  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
956  return;
957  }
958 
959  int errno = 0;
960  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
961  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
962  }
963  else if (state.ned_initialized_i) {
964  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
965  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
966  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
967  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
968  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
969  SetBit(state.accel_status, ACCEL_ECEF_I);
970  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
971  } else { /* could not get this representation, set errno */
972  errno = 1;
973  }
974  } else { /* ned coordinate system not initialized, set errno */
975  errno = 2;
976  }
977  if (errno) {
978  //struct NedCoor_i _ned_zero = {0};
979  //return _ned_zero;
980  }
981  /* set bit to indicate this representation is computed */
982  SetBit(state.accel_status, ACCEL_NED_I);
983 }
984 
985 void stateCalcAccelEcef_i(void)
986 {
987  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
988  return;
989  }
990 
991  int errno = 0;
992  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
993  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
994  }
995  else if (state.ned_initialized_i) {
996  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
997  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
998  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
999  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1000  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1001  SetBit(state.accel_status, ACCEL_NED_I);
1002  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1003  } else {
1004  /* could not get this representation, set errno */
1005  errno = 1;
1006  }
1007  } else { /* ned coordinate system not initialized, set errno */
1008  errno = 2;
1009  }
1010  if (errno) {}
1011  /* set bit to indicate this representation is computed */
1012  SetBit(state.accel_status, ACCEL_ECEF_I);
1013 }
1014 
1015 void stateCalcAccelNed_f(void)
1016 {
1017  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1018  return;
1019  }
1020 
1021  int errno = 0;
1022  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1023  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1024  }
1025  else if (state.ned_initialized_f) {
1026  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1027  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1028  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1029  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1030  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
1031  SetBit(state.accel_status, ACCEL_ECEF_F);
1032  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1033  } else { /* could not get this representation, set errno */
1034  errno = 1;
1035  }
1036  } else { /* ned coordinate system not initialized, set errno */
1037  errno = 2;
1038  }
1039  if (errno) {
1040  //struct NedCoor_f _ned_zero = {0.0f};
1041  //return _ned_zero;
1042  }
1043  /* set bit to indicate this representation is computed */
1044  SetBit(state.accel_status, ACCEL_NED_F);
1045 }
1046 
1047 void stateCalcAccelEcef_f(void)
1048 {
1049  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1050  return;
1051  }
1052 
1053  int errno = 0;
1054  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1055  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
1056  }
1057  else if (state.ned_initialized_f) {
1058  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1059  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1060  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1061  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1062  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1063  SetBit(state.accel_status, ACCEL_NED_F);
1064  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1065  } else {
1066  /* could not get this representation, set errno */
1067  errno = 1;
1068  }
1069  } else { /* ned coordinate system not initialized, set errno */
1070  errno = 2;
1071  }
1072  if (errno) {}
1073  /* set bit to indicate this representation is computed */
1074  SetBit(state.accel_status, ACCEL_ECEF_F);
1075 }
1078 /******************************************************************************
1079  * *
1080  * Transformation functions for the ANGULAR RATE representations *
1081  * *
1082  *****************************************************************************/
1086 void stateCalcBodyRates_i(void)
1087 {
1088  if (bit_is_set(state.rate_status, RATE_I)) {
1089  return;
1090  }
1091 
1092  if (bit_is_set(state.rate_status, RATE_F)) {
1093  RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
1094  }
1095  /* set bit to indicate this representation is computed */
1096  SetBit(state.rate_status, RATE_I);
1097 }
1098 
1099 void stateCalcBodyRates_f(void)
1100 {
1101  if (bit_is_set(state.rate_status, RATE_F)) {
1102  return;
1103  }
1104 
1105  if (bit_is_set(state.rate_status, RATE_I)) {
1106  RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
1107  }
1108  /* set bit to indicate this representation is computed */
1109  SetBit(state.rate_status, RATE_F);
1110 }
1111 
1115 /******************************************************************************
1116  * *
1117  * Transformation functions for the WIND- AND AIRSPEED representations *
1118  * *
1119  *****************************************************************************/
1123 void stateCalcHorizontalWindspeed_i(void)
1124 {
1125  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1126  return;
1127  }
1128 
1129  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1130  state.windspeed_i.vect2.x = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.x);
1131  state.windspeed_i.vect2.y = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.y);
1132  }
1133  /* set bit to indicate this representation is computed */
1134  SetBit(state.wind_air_status , WINDSPEED_I);
1135 }
1136 
1137 void stateCalcVerticalWindspeed_i(void)
1138 {
1139  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1140  return;
1141  }
1142 
1143  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1144  state.windspeed_i.vect3.z = SPEED_BFP_OF_REAL(state.windspeed_f.vect3.z);
1145  }
1146  /* set bit to indicate this representation is computed */
1147  SetBit(state.wind_air_status, DOWNWIND_I);
1148 }
1149 
1150 void stateCalcAirspeed_i(void)
1151 {
1152  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1153  return;
1154  }
1155 
1156  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1157  state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f);
1158  }
1159  /* set bit to indicate this representation is computed */
1160  SetBit(state.wind_air_status, AIRSPEED_I);
1161 }
1162 
1163 void stateCalcHorizontalWindspeed_f(void)
1164 {
1165  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1166  return;
1167  }
1168 
1169  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1170  state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.x);
1171  state.windspeed_f.vect2.y = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.y);
1172  }
1173  /* set bit to indicate this representation is computed */
1174  SetBit(state.wind_air_status, WINDSPEED_F);
1175 }
1176 
1177 void stateCalcVerticalWindspeed_f(void)
1178 {
1179  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1180  return;
1181  }
1182 
1183  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1184  state.windspeed_f.vect3.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect3.z);
1185  }
1186  /* set bit to indicate this representation is computed */
1187  SetBit(state.wind_air_status, DOWNWIND_F);
1188 }
1189 
1190 void stateCalcAirspeed_f(void)
1191 {
1192  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1193  return;
1194  }
1195 
1196  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1197  state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i);
1198  }
1199  /* set bit to indicate this representation is computed */
1200  SetBit(state.wind_air_status, AIRSPEED_F);
1201 }
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:733
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:709
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:1137
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:953
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:1123
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:1150
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:819
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
POS_NED_F
#define POS_NED_F
Definition: state.h:73
stateCalcPositionEcef_f
void stateCalcPositionEcef_f(void)
Definition: state.c:352
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:1086
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:727
POS_NED_I
#define POS_NED_I
Definition: state.h:68
stateCalcHorizontalSpeedNorm_i
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:696
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:770
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:745
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:621
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:118
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:703
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:892
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:178
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:263
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:672
stateCalcPositionUtm_f
void stateCalcPositionUtm_f(void)
Definition: state.c:316
stateCalcPositionNed_f
void stateCalcPositionNed_f(void)
Definition: state.c:382
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:91
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:741
State::windspeed_f
union State::@323 windspeed_f
Horizontal windspeed.
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:1047
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:1015
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:739
stateCalcBodyRates_f
void stateCalcBodyRates_f(void)
Definition: state.c:1099
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:985
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
State::windspeed_i
union State::@322 windspeed_i
Horizontal windspeed in north/east/down.
WINDSPEED_F
#define WINDSPEED_F
Definition: state.h:123
stateCalcSpeedEcef_f
void stateCalcSpeedEcef_f(void)
Definition: state.c:868
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:448
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:917
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:99
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:1190
stateCalcSpeedNed_i
void stateCalcSpeedNed_i(void)
Definition: state.c:572
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:520
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:73
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:85
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:1177
SPEED_HNORM_F
#define SPEED_HNORM_F
Definition: state.h:93
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:891
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
stateCalcHorizontalWindspeed_f
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1163
NED_OF_UTM_DIFF
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:79
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