Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
38 #if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
39 /* Preflight checks */
40 #include "modules/checks/preflight_checks.h"
41 static struct preflight_check_t state_pfc;
42 
43 static void state_preflight(struct preflight_result_t *result) {
44  if(!stateIsAttitudeValid()) {
45  preflight_error(result, "State attitude is invalid");
46  } else {
47  preflight_success(result, "State attitude is valid");
48  }
49 }
50 #endif // PREFLIGHT_CHECKS
51 
57 void stateInit(void)
58 {
59  state.pos_status = 0;
60  state.speed_status = 0;
61  state.accel_status = 0;
62  state.ned_to_body_orientation.status = 0;
63  state.rate_status = 0;
64  state.wind_air_status = 0;
65  state.ned_initialized_i = false;
66  state.ned_initialized_f = false;
67  state.utm_initialized_f = false;
68 
69  /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
70  state.utm_origin_f.zone = 0;
71 
72  /* Register preflight checks */
73 #if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
74  preflight_check_register(&state_pfc, state_preflight);
75 #endif
76 }
77 
78 
79 /*******************************************************************************
80  * *
81  * transformation functions for the POSITION representations *
82  * *
83  ******************************************************************************/
87 void stateCalcPositionEcef_i(void)
88 {
89  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
90  return;
91  }
92 
93  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
94  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
95  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
96  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
97  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
98  /* transform ned_f to ecef_f, set status bit, then convert to int */
99  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
100  SetBit(state.pos_status, POS_ECEF_F);
101  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
102  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
103  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i);
104  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
105  /* transform lla_f to ecef_f, set status bit, then convert to int */
106  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
107  SetBit(state.pos_status, POS_ECEF_F);
108  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
109  } else {
110  /* could not get this representation, set errno */
111  //struct EcefCoor_i _ecef_zero = {0};
112  //return _ecef_zero;
113  return;
114  }
115  /* set bit to indicate this representation is computed */
116  SetBit(state.pos_status, POS_ECEF_I);
117 }
118 
119 void stateCalcPositionNed_i(void)
120 {
121  if (bit_is_set(state.pos_status, POS_NED_I)) {
122  return;
123  }
124 
125  int errno = 0;
126  if (state.ned_initialized_i) {
127  if (bit_is_set(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_ENU_I)) {
130  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
131  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
132  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
133  SetBit(state.pos_status, POS_ENU_I);
134  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
135  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
136  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
137  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
138  /* transform ecef_f -> ned_f, set status bit, then convert to int */
139  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
140  SetBit(state.pos_status, POS_NED_F);
141  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
142  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
143  /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
144  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
145  SetBit(state.pos_status, POS_ECEF_F);
146  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_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_I)) {
150  ned_of_lla_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
151  } else { /* could not get this representation, set errno */
152  errno = 1;
153  }
154  } else if (state.utm_initialized_f) {
155  if (bit_is_set(state.pos_status, POS_NED_F)) {
156  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
157  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
158  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
159  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
160  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
161  SetBit(state.pos_status, POS_ENU_I);
162  INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
163  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
164  /* transform utm_f -> ned_f -> ned_i, set status bits */
165  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
166  SetBit(state.pos_status, POS_NED_F);
167  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
168  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
169  /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
170  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
171  SetBit(state.pos_status, POS_UTM_F);
172  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
173  SetBit(state.pos_status, POS_NED_F);
174  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
175  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
176  /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
177  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
178  SetBit(state.pos_status, POS_LLA_F);
179  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
180  SetBit(state.pos_status, POS_UTM_F);
181  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
182  SetBit(state.pos_status, POS_NED_F);
183  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
184  } else { /* could not get this representation, set errno */
185  errno = 2;
186  }
187  } else { /* ned coordinate system not initialized, set errno */
188  errno = 3;
189  }
190  if (errno) {
191  //struct NedCoor_i _ned_zero = {0};
192  //return _ned_zero;
193  return;
194  }
195  /* set bit to indicate this representation is computed */
196  SetBit(state.pos_status, POS_NED_I);
197 }
198 
199 void stateCalcPositionEnu_i(void)
200 {
201  if (bit_is_set(state.pos_status, POS_ENU_I)) {
202  return;
203  }
204 
205  int errno = 0;
206  if (state.ned_initialized_i) {
207  if (bit_is_set(state.pos_status, POS_NED_I)) {
208  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
209  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
210  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
211  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
212  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
213  SetBit(state.pos_status, POS_NED_I);
214  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
215  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
216  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
217  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
218  /* transform ecef_f -> enu_f, set status bit, then convert to int */
219  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
220  SetBit(state.pos_status, POS_ENU_F);
221  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
222  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
223  /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */
224  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
225  SetBit(state.pos_status, POS_ECEF_F);
226  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_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_I)) {
230  enu_of_lla_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
231  } else { /* could not get this representation, set errno */
232  errno = 1;
233  }
234  } else if (state.utm_initialized_f) {
235  if (bit_is_set(state.pos_status, POS_ENU_F)) {
236  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
237  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
238  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
239  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
240  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
241  SetBit(state.pos_status, POS_NED_I);
242  INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
243  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
244  /* transform utm_f -> enu_f -> enu_i , set status bits */
245  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
246  SetBit(state.pos_status, POS_ENU_F);
247  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
248  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
249  /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */
250  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
251  SetBit(state.pos_status, POS_UTM_F);
252  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
253  SetBit(state.pos_status, POS_ENU_F);
254  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
255  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
256  /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */
257  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
258  SetBit(state.pos_status, POS_LLA_F);
259  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
260  SetBit(state.pos_status, POS_UTM_F);
261  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
262  SetBit(state.pos_status, POS_ENU_F);
263  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
264  } else { /* could not get this representation, set errno */
265  errno = 2;
266  }
267  } else { /* ned coordinate system not initialized, set errno */
268  errno = 3;
269  }
270  if (errno) {
271  //struct EnuCoor_i _enu_zero = {0};
272  //return _enu_zero;
273  return;
274  }
275  /* set bit to indicate this representation is computed */
276  SetBit(state.pos_status, POS_ENU_I);
277 }
278 
286 void stateCalcPositionLla_i(void)
287 {
288  if (bit_is_set(state.pos_status, POS_LLA_I)) {
289  return;
290  }
291 
292  int errno = 0;
293  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
294  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
295  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
296  /* transform ecef_f -> ecef_i -> lla_i, set status bits */
297  ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
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_UTM_F)) {
301  /* transform utm_f -> lla_f -> lla_i, set status bits */
302  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
303  SetBit(state.pos_status, POS_LLA_F);
304  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
305  } else if (state.ned_initialized_i) {
306  if (bit_is_set(state.pos_status, POS_NED_I)) {
307  /* transform ned_i -> ecef_i -> lla_i, set status bits */
308  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_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 if (bit_is_set(state.pos_status, POS_ENU_I)) {
312  /* transform enu_i -> ecef_i -> lla_i, set status bits */
313  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
314  SetBit(state.pos_status, POS_ECEF_I);
315  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
316  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
317  /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
318  NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
319  SetBit(state.pos_status, POS_NED_I);
320  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
321  SetBit(state.pos_status, POS_ECEF_I);
322  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
323  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
324  /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
325  ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
326  SetBit(state.pos_status, POS_ENU_I);
327  ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
328  SetBit(state.pos_status, POS_ECEF_I);
329  lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
330  } else { /* could not get this representation, set errno */
331  errno = 1;
332  }
333  } else if (state.utm_initialized_f) {
334  if (bit_is_set(state.pos_status, POS_NED_I)) {
335  /* transform ned_i -> ned_f -> utm_f -> lla_f -> lla_i, set status bits */
336  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
337  SetBit(state.pos_status, POS_NED_F);
338  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
339  SetBit(state.pos_status, POS_UTM_F);
340  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
341  SetBit(state.pos_status, POS_LLA_F);
342  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
343  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
344  /* transform enu_i -> enu_f -> utm_f -> lla_f -> lla_i, set status bits */
345  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
346  SetBit(state.pos_status, POS_ENU_F);
347  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
348  SetBit(state.pos_status, POS_UTM_F);
349  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
350  SetBit(state.pos_status, POS_LLA_F);
351  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
352  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
353  /* transform ned_f -> utm_f -> lla_f -> lla_i, set status bits */
354  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
355  SetBit(state.pos_status, POS_UTM_F);
356  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
357  SetBit(state.pos_status, POS_LLA_F);
358  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
359  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
360  /* transform enu_f -> utm_f -> lla_f -> lla_i, set status bits */
361  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
362  SetBit(state.pos_status, POS_UTM_F);
363  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
364  SetBit(state.pos_status, POS_LLA_F);
365  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
366  } else { /* could not get this representation, set errno */
367  errno = 2;
368  }
369  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
370  LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
371  } else { /* ned coordinate system not initialized, set errno */
372  errno = 3;
373  }
374  if (errno) {
375  /* could not get this representation, set errno */
376  //struct LlaCoor_i _lla_zero = {0};
377  //return _lla_zero;
378  return;
379  }
380  /* set bit to indicate this representation is computed */
381  SetBit(state.pos_status, POS_LLA_I);
382 }
383 
384 void stateCalcPositionUtm_f(void)
385 {
386  if (bit_is_set(state.pos_status, POS_UTM_F)) {
387  return;
388  }
389 
390  if (bit_is_set(state.pos_status, POS_LLA_F)) {
391  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
392  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
393  /* transform lla_i -> lla_f -> utm_f, set status bits */
394  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
395  SetBit(state.pos_status, POS_LLA_F);
396  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
397  } else if (state.utm_initialized_f) {
398  if (bit_is_set(state.pos_status, POS_ENU_F)) {
399  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
400  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
401  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
402  SetBit(state.pos_status, POS_ENU_F);
403  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
404  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
405  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
406  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
407  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
408  SetBit(state.pos_status, POS_NED_F);
409  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
410  }
411  } else {
412  /* could not get this representation, set errno */
413  //struct EcefCoor_f _ecef_zero = {0.0f};
414  //return _ecef_zero;
415  return;
416  }
417  /* set bit to indicate this representation is computed */
418  SetBit(state.pos_status, POS_UTM_F);
419 }
420 
421 void stateCalcPositionEcef_f(void)
422 {
423  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
424  return;
425  }
426 
427  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
428  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
429  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
430  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
431  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
432  /* transform ned_i -> ecef_i -> ecef_f, set status bits */
433  ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
434  SetBit(state.pos_status, POS_ECEF_F);
435  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
436  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
437  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
438  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
439  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
440  SetBit(state.pos_status, POS_LLA_F);
441  ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
442  } else {
443  /* could not get this representation, set errno */
444  //struct EcefCoor_f _ecef_zero = {0.0f};
445  //return _ecef_zero;
446  return;
447  }
448  /* set bit to indicate this representation is computed */
449  SetBit(state.pos_status, POS_ECEF_F);
450 }
451 
452 void stateCalcPositionNed_f(void)
453 {
454  if (bit_is_set(state.pos_status, POS_NED_F)) {
455  return;
456  }
457 
458  int errno = 0;
459  if (state.ned_initialized_f) {
460  if (bit_is_set(state.pos_status, POS_NED_I)) {
461  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
462  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
463  ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
464  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
465  /* transform ecef_i -> ned_i -> ned_f, set status bits */
466  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
467  SetBit(state.pos_status, POS_NED_I);
468  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
469  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
470  ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f);
471  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
472  /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
473  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
474  SetBit(state.pos_status, POS_ECEF_I);
475  ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
476  SetBit(state.pos_status, POS_NED_I);
477  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
478  } else { /* could not get this representation, set errno */
479  errno = 1;
480  }
481  } else if (state.utm_initialized_f) {
482  if (bit_is_set(state.pos_status, POS_NED_I)) {
483  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
484  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
485  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
486  SetBit(state.pos_status, POS_ENU_F);
487  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
488  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
489  VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f);
490  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
491  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
492  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
493  /* transform lla_f -> utm_f -> ned, set status bits */
494  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
495  SetBit(state.pos_status, POS_UTM_F);
496  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
497  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
498  /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */
499  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
500  SetBit(state.pos_status, POS_LLA_F);
501  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
502  SetBit(state.pos_status, POS_UTM_F);
503  NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
504  } else { /* could not get this representation, set errno */
505  errno = 2;
506  }
507  } else { /* ned coordinate system not initialized, set errno */
508  errno = 3;
509  }
510  if (errno) {
511  //struct NedCoor_f _ned_zero = {0.0f};
512  //return _ned_zero;
513  return;
514  }
515  /* set bit to indicate this representation is computed */
516  SetBit(state.pos_status, POS_NED_F);
517 }
518 
519 void stateCalcPositionEnu_f(void)
520 {
521  if (bit_is_set(state.pos_status, POS_ENU_F)) {
522  return;
523  }
524 
525  int errno = 0;
526  if (state.ned_initialized_f) {
527  if (bit_is_set(state.pos_status, POS_NED_F)) {
528  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
529  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
530  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
531  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
532  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
533  SetBit(state.pos_status, POS_NED_F);
534  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
535  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
536  enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
537  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
538  /* transform ecef_i -> enu_i -> enu_f, set status bits */
539  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
540  SetBit(state.pos_status, POS_ENU_I);
541  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
542  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
543  enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
544  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
545  /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
546  ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
547  SetBit(state.pos_status, POS_ECEF_I);
548  enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
549  SetBit(state.pos_status, POS_ENU_I);
550  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
551  } else { /* could not get this representation, set errno */
552  errno = 1;
553  }
554  } else if (state.utm_initialized_f) {
555  if (bit_is_set(state.pos_status, POS_ENU_I)) {
556  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
557  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
558  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
559  } else if (bit_is_set(state.pos_status, POS_NED_I)) {
560  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
561  SetBit(state.pos_status, POS_NED_F);
562  VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
563  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
564  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
565  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
566  /* transform lla_f -> utm_f -> enu, set status bits */
567  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
568  SetBit(state.pos_status, POS_UTM_F);
569  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
570  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
571  /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
572  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
573  SetBit(state.pos_status, POS_LLA_F);
574  utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
575  SetBit(state.pos_status, POS_UTM_F);
576  ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
577  } else { /* could not get this representation, set errno */
578  errno = 2;
579  }
580  } else { /* ned coordinate system not initialized, set errno */
581  errno = 3;
582  }
583  if (errno) {
584  //struct EnuCoor_f _enu_zero = {0.0f};
585  //return _enu_zero;
586  return;
587  }
588  /* set bit to indicate this representation is computed */
589  SetBit(state.pos_status, POS_ENU_F);
590 }
591 
592 void stateCalcPositionLla_f(void)
593 {
594  if (bit_is_set(state.pos_status, POS_LLA_F)) {
595  return;
596  }
597 
598  int errno = 0;
599  if (bit_is_set(state.pos_status, POS_LLA_I)) {
600  LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
601  } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
602  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
603  } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
604  /* transform ecef_i -> ecef_f -> lla_f, set status bits */
605  ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
606  SetBit(state.pos_status, POS_ECEF_F);
607  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
608  } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
609  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
610  } else if (state.ned_initialized_f) {
611  if (bit_is_set(state.pos_status, POS_NED_F)) {
612  /* transform ned_f -> ecef_f -> lla_f, set status bits */
613  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_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 if (bit_is_set(state.pos_status, POS_NED_I)) {
617  /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
618  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
619  SetBit(state.pos_status, POS_NED_F);
620  ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
621  SetBit(state.pos_status, POS_ECEF_F);
622  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
623  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
624  /* transform enu_f -> ecef_f -> lla_f, set status bits */
625  ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
626  SetBit(state.pos_status, POS_ECEF_F);
627  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
628  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
629  /* transform enu_i -> enu_f -> ecef_f -> lla_f, set status bits */
630  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
631  SetBit(state.pos_status, POS_ENU_F);
632  ecef_of_enu_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.enu_pos_f);
633  SetBit(state.pos_status, POS_ECEF_F);
634  lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
635  } else { /* could not get this representation, set errno */
636  errno = 1;
637  }
638  } else if (state.utm_initialized_f) {
639  if (bit_is_set(state.pos_status, POS_NED_I)) {
640  /* transform ned_i -> ned_f -> utm_f -> lla_f, set status bits */
641  NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
642  SetBit(state.pos_status, POS_NED_F);
643  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
644  SetBit(state.pos_status, POS_UTM_F);
645  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
646  } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
647  /* transform enu_i -> enu_f -> utm_f -> lla_f, set status bits */
648  ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
649  SetBit(state.pos_status, POS_ENU_F);
650  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
651  SetBit(state.pos_status, POS_UTM_F);
652  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
653  } else if (bit_is_set(state.pos_status, POS_NED_F)) {
654  /* transform ned_f -> utm_f -> lla_f, set status bits */
655  UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
656  SetBit(state.pos_status, POS_UTM_F);
657  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
658  } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
659  /* transform enu_f -> utm_f -> lla_f, set status bits */
660  UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
661  SetBit(state.pos_status, POS_UTM_F);
662  lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
663  } else { /* could not get this representation, set errno */
664  errno = 2;
665  }
666  } else { /* ned coordinate system not initialized, set errno */
667  errno = 3;
668  }
669  if (errno) {
670  /* could not get this representation, set errno */
671  //struct LlaCoor_f _lla_zero = {0.0};
672  //return _lla_zero;
673  return;
674  }
675  /* set bit to indicate this representation is computed */
676  SetBit(state.pos_status, POS_LLA_F);
677 }
684 /******************************************************************************
685  * *
686  * Transformation functions for the SPEED representations *
687  * *
688  *****************************************************************************/
691 /************************ Set functions ****************************/
692 
693 void stateCalcSpeedNed_i(void)
694 {
695  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
696  return;
697  }
698 
699  int errno = 0;
700  if (state.ned_initialized_i) {
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 if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
710  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
711  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
712  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
713  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
714  SetBit(state.speed_status, SPEED_ECEF_I);
715  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
716  } else { /* could not get this representation, set errno */
717  errno = 1;
718  }
719  } else if (state.utm_initialized_f) {
720  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
721  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
722  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
723  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
724  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
725  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
726  SetBit(state.speed_status, SPEED_ENU_I);
727  INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i);
728  } else { /* could not get this representation, set errno */
729  errno = 2;
730  }
731  } else { /* ned coordinate system not initialized, set errno */
732  errno = 3;
733  }
734  if (errno) {
735  //struct NedCoor_i _ned_zero = {0};
736  //return _ned_zero;
737  return;
738  }
739  /* set bit to indicate this representation is computed */
740  SetBit(state.speed_status, SPEED_NED_I);
741 }
742 
743 void stateCalcSpeedEnu_i(void)
744 {
745  if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
746  return;
747  }
748 
749  int errno = 0;
750  if (state.ned_initialized_i) {
751  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
752  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
753  }
754  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
755  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
756  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
757  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
758  SetBit(state.speed_status, SPEED_NED_I);
759  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
760  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
761  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
762  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
763  /* transform ecef_f -> ecef_i -> enu_i , set status bits */
764  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
765  SetBit(state.speed_status, SPEED_ECEF_I);
766  enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
767  } else { /* could not get this representation, set errno */
768  errno = 1;
769  }
770  } else if (state.utm_initialized_f) {
771  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
772  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
773  }
774  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
775  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
776  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
777  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
778  SetBit(state.speed_status, SPEED_NED_I);
779  INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i);
780  } else { /* could not get this representation, set errno */
781  errno = 2;
782  }
783  } else { /* ned coordinate system not initialized, set errno */
784  errno = 3;
785  }
786  if (errno) {
787  //struct EnuCoor_i _enu_zero = {0};
788  //return _enu_zero;
789  return;
790  }
791  /* set bit to indicate this representation is computed */
792  SetBit(state.speed_status, SPEED_ENU_I);
793 }
794 
795 void stateCalcSpeedEcef_i(void)
796 {
797  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
798  return;
799  }
800 
801  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
802  SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f);
803  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
804  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
805  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
806  /* transform ned_f -> ned_i -> ecef_i , set status bits */
807  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
808  SetBit(state.speed_status, SPEED_NED_I);
809  ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i);
810  } else {
811  /* could not get this representation, set errno */
812  //struct EcefCoor_i _ecef_zero = {0};
813  //return _ecef_zero;
814  return;
815  }
816  /* set bit to indicate this representation is computed */
817  SetBit(state.speed_status, SPEED_ECEF_I);
818 }
819 
820 void stateCalcHorizontalSpeedNorm_i(void)
821 {
822  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
823  return;
824  }
825 
826  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
827  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
828  } else if (bit_is_set(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_NED_F)) {
833  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
834  SetBit(state.speed_status, SPEED_HNORM_F);
835  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
836  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
837  uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x +
838  state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC;
839  state.h_speed_norm_i = int32_sqrt(n2);
840  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
841  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
842  SetBit(state.speed_status, SPEED_HNORM_F);
843  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
844  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
845  /* transform ecef speed to ned, set status bit, then compute norm */
846  ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
847  SetBit(state.speed_status, SPEED_NED_I);
848  uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
849  state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
850  state.h_speed_norm_i = int32_sqrt(n2);
851  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
852  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
853  SetBit(state.speed_status, SPEED_NED_F);
854  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
855  SetBit(state.speed_status, SPEED_HNORM_F);
856  state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
857  } else {
858  //int32_t _norm_zero = 0;
859  //return _norm_zero;
860  return;
861  }
862  /* set bit to indicate this representation is computed */
863  SetBit(state.speed_status, SPEED_HNORM_I);
864 }
865 
866 void stateCalcHorizontalSpeedDir_i(void)
867 {
868  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
869  return;
870  }
871 
872  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
873  state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
874  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
875  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
876  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
877  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
878  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
879  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
880  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
881  SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
882  SetBit(state.speed_status, SPEED_NED_I);
883  state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x);
884  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
885  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
886  SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
887  SetBit(state.speed_status, SPEED_ENU_I);
888  state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y);
889  INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
890  } else {
891  return;
892  }
893  /* set bit to indicate this representation is computed */
894  SetBit(state.speed_status, SPEED_HDIR_I);
895 }
896 
897 void stateCalcSpeedNed_f(void)
898 {
899  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
900  return;
901  }
902 
903  int errno = 0;
904  if (state.ned_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 if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
914  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
915  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
916  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
917  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
918  SetBit(state.speed_status, SPEED_ECEF_F);
919  ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
920  } else { /* could not get this representation, set errno */
921  errno = 1;
922  }
923  } else if (state.utm_initialized_f) {
924  if (bit_is_set(state.speed_status, SPEED_NED_I)) {
925  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
926  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
927  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
928  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
929  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
930  SetBit(state.speed_status, SPEED_ENU_F);
931  VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f);
932  } else { /* could not get this representation, set errno */
933  errno = 2;
934  }
935  } else { /* ned coordinate system not initialized, set errno */
936  errno = 3;
937  }
938  if (errno) {
939  //struct NedCoor_f _ned_zero = {0.0f};
940  //return _ned_zero;
941  return;
942  }
943  /* set bit to indicate this representation is computed */
944  SetBit(state.speed_status, SPEED_NED_F);
945 }
946 
947 void stateCalcSpeedEnu_f(void)
948 {
949  if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
950  return;
951  }
952 
953  int errno = 0;
954  if (state.ned_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  SPEEDS_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.speed_status, SPEED_NED_F);
962  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
963  } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
964  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
965  } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
966  /* transform ecef_I -> ecef_f -> enu_f , set status bits */
967  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i);
968  SetBit(state.speed_status, SPEED_ECEF_F);
969  enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
970  } else { /* could not get this representation, set errno */
971  errno = 1;
972  }
973  } else if (state.utm_initialized_f) {
974  if (bit_is_set(state.speed_status, SPEED_NED_F)) {
975  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
976  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
977  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
978  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
979  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
980  SetBit(state.speed_status, SPEED_NED_F);
981  VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f);
982  } else { /* could not get this representation, set errno */
983  errno = 2;
984  }
985  } else { /* ned coordinate system not initialized, set errno */
986  errno = 3;
987  }
988  if (errno) {
989  //struct EnuCoor_f _enu_zero = {0};
990  //return _enu_zero;
991  return;
992  }
993  /* set bit to indicate this representation is computed */
994  SetBit(state.speed_status, SPEED_ENU_F);
995 }
996 
997 void stateCalcSpeedEcef_f(void)
998 {
999  if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1000  return;
1001  }
1002 
1003  if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1004  SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i);
1005  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1006  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
1007  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1008  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1009  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1010  SetBit(state.speed_status, SPEED_NED_F);
1011  ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f);
1012  } else {
1013  /* could not get this representation, set errno */
1014  //struct EcefCoor_f _ecef_zero = {0.0f};
1015  //return _ecef_zero;
1016  return;
1017  }
1018  /* set bit to indicate this representation is computed */
1019  SetBit(state.speed_status, SPEED_ECEF_F);
1020 }
1021 
1022 void stateCalcHorizontalSpeedNorm_f(void)
1023 {
1024  if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
1025  return;
1026  }
1027 
1028  if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
1029  state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
1030  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1031  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1032  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1033  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
1034  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1035  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1036  SetBit(state.speed_status, SPEED_NED_F);
1037  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
1038  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1039  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1040  SetBit(state.speed_status, SPEED_ENU_F);
1041  state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
1042  } else {
1043  return;
1044  }
1045  /* set bit to indicate this representation is computed */
1046  SetBit(state.speed_status, SPEED_HNORM_F);
1047 }
1048 
1049 void stateCalcHorizontalSpeedDir_f(void)
1050 {
1051  if (bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1052  return;
1053  }
1054 
1055  if (bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1056  state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i);
1057  } else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
1058  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1059  } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
1060  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
1061  } else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
1062  SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
1063  SetBit(state.speed_status, SPEED_NED_F);
1064  state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x);
1065  } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
1066  SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
1067  SetBit(state.speed_status, SPEED_ENU_F);
1068  state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y);
1069  } else {
1070  return;
1071  }
1072  /* set bit to indicate this representation is computed */
1073  SetBit(state.speed_status, SPEED_HDIR_F);
1074 }
1079 /******************************************************************************
1080  * *
1081  * Transformation functions for the ACCELERATION representations *
1082  * *
1083  *****************************************************************************/
1087 void stateCalcAccelNed_i(void)
1088 {
1089  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1090  return;
1091  }
1092 
1093  int errno = 0;
1094  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1095  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1096  }
1097  else if (state.ned_initialized_i) {
1098  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1099  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
1100  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1101  /* transform ecef_f -> ecef_i -> ned_i , set status bits */
1102  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1103  SetBit(state.accel_status, ACCEL_ECEF_I);
1104  ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
1105  } else { /* could not get this representation, set errno */
1106  errno = 1;
1107  }
1108  } else { /* ned coordinate system not initialized, set errno */
1109  errno = 2;
1110  }
1111  if (errno) {
1112  //struct NedCoor_i _ned_zero = {0};
1113  //return _ned_zero;
1114  return;
1115  }
1116  /* set bit to indicate this representation is computed */
1117  SetBit(state.accel_status, ACCEL_NED_I);
1118 }
1119 
1120 void stateCalcAccelEcef_i(void)
1121 {
1122  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1123  return;
1124  }
1125 
1126  int errno = 0;
1127  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1128  ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
1129  }
1130  else if (state.ned_initialized_i) {
1131  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1132  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1133  } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1134  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1135  ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
1136  SetBit(state.accel_status, ACCEL_NED_I);
1137  ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
1138  } else {
1139  /* could not get this representation, set errno */
1140  errno = 1;
1141  }
1142  } else { /* ned coordinate system not initialized, set errno */
1143  errno = 2;
1144  }
1145  if (errno) {
1146  return;
1147  }
1148  /* set bit to indicate this representation is computed */
1149  SetBit(state.accel_status, ACCEL_ECEF_I);
1150 }
1151 
1152 void stateCalcAccelNed_f(void)
1153 {
1154  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1155  return;
1156  }
1157 
1158  int errno = 0;
1159  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1160  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1161  }
1162  else if (state.ned_initialized_f) {
1163  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1164  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1165  } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1166  /* transform ecef_i -> ecef_f -> ned_f , set status bits */
1167  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
1168  SetBit(state.accel_status, ACCEL_ECEF_F);
1169  ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
1170  } else { /* could not get this representation, set errno */
1171  errno = 1;
1172  }
1173  } else { /* ned coordinate system not initialized, set errno */
1174  errno = 2;
1175  }
1176  if (errno) {
1177  //struct NedCoor_f _ned_zero = {0.0f};
1178  //return _ned_zero;
1179  return;
1180  }
1181  /* set bit to indicate this representation is computed */
1182  SetBit(state.accel_status, ACCEL_NED_F);
1183 }
1184 
1185 void stateCalcAccelEcef_f(void)
1186 {
1187  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1188  return;
1189  }
1190 
1191  int errno = 0;
1192  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1193  ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
1194  }
1195  else if (state.ned_initialized_f) {
1196  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
1197  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1198  } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
1199  /* transform ned_f -> ned_i -> ecef_i , set status bits */
1200  ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
1201  SetBit(state.accel_status, ACCEL_NED_F);
1202  ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
1203  } else {
1204  /* could not get this representation, set errno */
1205  errno = 1;
1206  }
1207  } else { /* ned coordinate system not initialized, set errno */
1208  errno = 2;
1209  }
1210  if (errno) {
1211  return;
1212  }
1213  /* set bit to indicate this representation is computed */
1214  SetBit(state.accel_status, ACCEL_ECEF_F);
1215 }
1218 /******************************************************************************
1219  * *
1220  * Transformation functions for the ANGULAR RATE representations *
1221  * *
1222  *****************************************************************************/
1226 void stateCalcBodyRates_i(void)
1227 {
1228  if (bit_is_set(state.rate_status, RATE_I)) {
1229  return;
1230  }
1231 
1232  if (bit_is_set(state.rate_status, RATE_F)) {
1233  RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f);
1234  } else {
1235  return;
1236  }
1237  /* set bit to indicate this representation is computed */
1238  SetBit(state.rate_status, RATE_I);
1239 }
1240 
1241 void stateCalcBodyRates_f(void)
1242 {
1243  if (bit_is_set(state.rate_status, RATE_F)) {
1244  return;
1245  }
1246 
1247  if (bit_is_set(state.rate_status, RATE_I)) {
1248  RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i);
1249  } else {
1250  return;
1251  }
1252  /* set bit to indicate this representation is computed */
1253  SetBit(state.rate_status, RATE_F);
1254 }
1255 
1259 /******************************************************************************
1260  * *
1261  * Transformation functions for the WIND- AND AIRSPEED representations *
1262  * *
1263  *****************************************************************************/
1267 void stateCalcHorizontalWindspeed_i(void)
1268 {
1269  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1270  return;
1271  }
1272 
1273  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1274  state.windspeed_i.vect2.x = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.x);
1275  state.windspeed_i.vect2.y = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.y);
1276  } else {
1277  return;
1278  }
1279  /* set bit to indicate this representation is computed */
1280  SetBit(state.wind_air_status , WINDSPEED_I);
1281 }
1282 
1283 void stateCalcVerticalWindspeed_i(void)
1284 {
1285  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1286  return;
1287  }
1288 
1289  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1290  state.windspeed_i.vect3.z = SPEED_BFP_OF_REAL(state.windspeed_f.vect3.z);
1291  } else {
1292  return;
1293  }
1294  /* set bit to indicate this representation is computed */
1295  SetBit(state.wind_air_status, DOWNWIND_I);
1296 }
1297 
1298 void stateCalcAirspeed_i(void)
1299 {
1300  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1301  return;
1302  }
1303 
1304  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1305  state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f);
1306  } else {
1307  return;
1308  }
1309  /* set bit to indicate this representation is computed */
1310  SetBit(state.wind_air_status, AIRSPEED_I);
1311 }
1312 
1313 void stateCalcHorizontalWindspeed_f(void)
1314 {
1315  if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1316  return;
1317  }
1318 
1319  if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1320  state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.x);
1321  state.windspeed_f.vect2.y = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.y);
1322  } else {
1323  return;
1324  }
1325  /* set bit to indicate this representation is computed */
1326  SetBit(state.wind_air_status, WINDSPEED_F);
1327 }
1328 
1329 void stateCalcVerticalWindspeed_f(void)
1330 {
1331  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1332  return;
1333  }
1334 
1335  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1336  state.windspeed_f.vect3.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect3.z);
1337  } else {
1338  return;
1339  }
1340  /* set bit to indicate this representation is computed */
1341  SetBit(state.wind_air_status, DOWNWIND_F);
1342 }
1343 
1344 void stateCalcAirspeed_f(void)
1345 {
1346  if (bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1347  return;
1348  }
1349 
1350  if (bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1351  state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i);
1352  } else {
1353  return;
1354  }
1355  /* set bit to indicate this representation is computed */
1356  SetBit(state.wind_air_status, AIRSPEED_F);
1357 }
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
SPEEDS_BFP_OF_REAL
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:789
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
SPEEDS_FLOAT_OF_BFP
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:783
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_sqrt
uint32_t int32_sqrt(uint32_t in)
Definition: pprz_algebra_int.c:30
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
INT32_COURSE_NORMALIZE
#define INT32_COURSE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:131
SPEED_FLOAT_OF_BFP
#define SPEED_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:220
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
UTM_OF_NED_ADD
#define UTM_OF_NED_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:96
ENU_OF_UTM_DIFF
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:78
NED_OF_UTM_DIFF
#define NED_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:84
UTM_OF_ENU_ADD
#define UTM_OF_ENU_ADD(_utm, _pos, _utm0)
Definition: pprz_geodetic.h:90
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
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
ECEF_FLOAT_OF_BFP
#define ECEF_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:173
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
ENU_FLOAT_OF_BFP
#define ENU_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:207
ECEF_BFP_OF_REAL
#define ECEF_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:172
ENU_BFP_OF_REAL
#define ENU_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:199
LLA_BFP_OF_REAL
#define LLA_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:176
NED_BFP_OF_REAL
#define NED_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:193
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
NED_FLOAT_OF_BFP
#define NED_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:201
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
INT32_VECT3_ENU_OF_NED
#define INT32_VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:152
VECT3_ENU_OF_NED
#define VECT3_ENU_OF_NED(_o, _i)
Definition: pprz_geodetic_int.h:144
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
VECT3_NED_OF_ENU
#define VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:150
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
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
LLA_FLOAT_OF_BFP
#define LLA_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:182
INT32_VECT3_NED_OF_ENU
#define INT32_VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:151
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
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
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
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
OrientationReps::status
uint8_t status
Holds the status bits for all orientation representations.
Definition: pprz_orientation_conversion.h:85
ACCEL_ECEF_I
#define ACCEL_ECEF_I
Definition: state.h:102
State::ecef_accel_f
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
State::ned_accel_i
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
ACCEL_NED_F
#define ACCEL_NED_F
Definition: state.h:105
State::accel_status
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
stateCalcAccelNed_i
void stateCalcAccelNed_i(void)
Definition: state.c:1087
stateCalcAccelEcef_i
void stateCalcAccelEcef_i(void)
Definition: state.c:1120
State::ecef_accel_i
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
ACCEL_ECEF_F
#define ACCEL_ECEF_F
Definition: state.h:104
stateCalcAccelEcef_f
void stateCalcAccelEcef_f(void)
Definition: state.c:1185
State::ned_accel_f
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
ACCEL_NED_I
#define ACCEL_NED_I
Definition: state.h:103
stateCalcAccelNed_f
void stateCalcAccelNed_f(void)
Definition: state.c:1152
stateIsAttitudeValid
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
State::ned_to_body_orientation
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
state
struct State state
Definition: state.c:36
stateInit
void stateInit(void)
Definition: state.c:57
State
Structure holding vehicle state data.
Definition: state.h:134
POS_LLA_F
#define POS_LLA_F
Definition: state.h:75
State::ned_pos_i
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
POS_UTM_F
#define POS_UTM_F
Definition: state.h:76
POS_LLA_I
#define POS_LLA_I
Definition: state.h:70
State::ned_pos_f
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:243
POS_ENU_F
#define POS_ENU_F
Definition: state.h:74
State::enu_pos_f
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
POS_ECEF_F
#define POS_ECEF_F
Definition: state.h:72
State::utm_pos_f
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
stateCalcPositionNed_f
void stateCalcPositionNed_f(void)
Definition: state.c:452
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
State::pos_status
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:144
State::lla_pos_i
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
stateCalcPositionUtm_f
void stateCalcPositionUtm_f(void)
Definition: state.c:384
stateCalcPositionEnu_f
void stateCalcPositionEnu_f(void)
Definition: state.c:519
State::ecef_pos_f
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
State::utm_initialized_f
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
POS_ENU_I
#define POS_ENU_I
Definition: state.h:69
POS_ECEF_I
#define POS_ECEF_I
Definition: state.h:67
stateCalcPositionEcef_f
void stateCalcPositionEcef_f(void)
Definition: state.c:421
State::enu_pos_i
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
POS_NED_F
#define POS_NED_F
Definition: state.h:73
stateCalcPositionLla_f
void stateCalcPositionLla_f(void)
Definition: state.c:592
stateCalcPositionEcef_i
void stateCalcPositionEcef_i(void)
Definition: state.c:87
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
stateCalcPositionNed_i
void stateCalcPositionNed_i(void)
Definition: state.c:119
stateCalcPositionLla_i
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:286
State::lla_pos_f
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
State::ecef_pos_i
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:150
POS_NED_I
#define POS_NED_I
Definition: state.h:68
stateCalcPositionEnu_i
void stateCalcPositionEnu_i(void)
Definition: state.c:199
stateCalcBodyRates_f
void stateCalcBodyRates_f(void)
Definition: state.c:1241
RATE_F
#define RATE_F
Definition: state.h:113
State::body_rates_f
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
RATE_I
#define RATE_I
Definition: state.h:112
stateCalcBodyRates_i
void stateCalcBodyRates_i(void)
Definition: state.c:1226
State::body_rates_i
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
State::rate_status
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
SPEED_ENU_F
#define SPEED_ENU_F
Definition: state.h:92
SPEED_ENU_I
#define SPEED_ENU_I
Definition: state.h:87
stateCalcSpeedEnu_f
void stateCalcSpeedEnu_f(void)
Definition: state.c:947
State::ned_speed_f
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
SPEED_ECEF_F
#define SPEED_ECEF_F
Definition: state.h:90
stateCalcSpeedNed_f
void stateCalcSpeedNed_f(void)
Definition: state.c:897
SPEED_ECEF_I
#define SPEED_ECEF_I
Definition: state.h:85
stateCalcHorizontalSpeedDir_i
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:866
SPEED_NED_F
#define SPEED_NED_F
Definition: state.h:91
State::ned_speed_i
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
State::h_speed_norm_f
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
State::h_speed_dir_i
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
State::h_speed_dir_f
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
stateCalcHorizontalSpeedNorm_f
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:1022
stateCalcHorizontalSpeedNorm_i
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:820
State::ecef_speed_f
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
stateCalcSpeedEcef_f
void stateCalcSpeedEcef_f(void)
Definition: state.c:997
SPEED_HDIR_I
#define SPEED_HDIR_I
Definition: state.h:89
stateCalcSpeedEnu_i
void stateCalcSpeedEnu_i(void)
Definition: state.c:743
SPEED_HNORM_F
#define SPEED_HNORM_F
Definition: state.h:93
SPEED_NED_I
#define SPEED_NED_I
Definition: state.h:86
State::ecef_speed_i
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
stateCalcHorizontalSpeedDir_f
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1049
State::enu_speed_i
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
State::speed_status
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
stateCalcSpeedNed_i
void stateCalcSpeedNed_i(void)
Definition: state.c:693
SPEED_HNORM_I
#define SPEED_HNORM_I
Definition: state.h:88
State::h_speed_norm_i
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
SPEED_HDIR_F
#define SPEED_HDIR_F
Definition: state.h:94
State::enu_speed_f
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
stateCalcSpeedEcef_i
void stateCalcSpeedEcef_i(void)
Definition: state.c:795
WINDSPEED_I
#define WINDSPEED_I
Definition: state.h:120
State::airspeed_i
int32_t airspeed_i
Norm of relative wind speed.
Definition: state.h:416
DOWNWIND_F
#define DOWNWIND_F
Definition: state.h:124
WINDSPEED_F
#define WINDSPEED_F
Definition: state.h:123
State::airspeed_f
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
State::wind_air_status
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
AIRSPEED_I
#define AIRSPEED_I
Definition: state.h:122
stateCalcAirspeed_i
void stateCalcAirspeed_i(void)
Definition: state.c:1298
State::windspeed_f
union State::@354 windspeed_f
Horizontal windspeed.
State::windspeed_i
union State::@353 windspeed_i
Horizontal windspeed in north/east/down.
stateCalcVerticalWindspeed_i
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1283
stateCalcAirspeed_f
void stateCalcAirspeed_f(void)
Definition: state.c:1344
AIRSPEED_F
#define AIRSPEED_F
Definition: state.h:125
stateCalcHorizontalWindspeed_f
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1313
stateCalcHorizontalWindspeed_i
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1267
DOWNWIND_I
#define DOWNWIND_I
Definition: state.h:121
stateCalcVerticalWindspeed_f
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1329
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
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
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
lla_of_utm_f
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
Definition: pprz_geodetic_float.c:344
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
ecef_of_lla_f
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
Definition: pprz_geodetic_float.c:241
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
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
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
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
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
lla_of_ecef_f
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
Definition: pprz_geodetic_float.c:204
utm_of_lla_f
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:308
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
int32_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:905
preflight_error
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
Definition: preflight_checks.c:173
preflight_success
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
Definition: preflight_checks.c:257
preflight_check_register
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Definition: preflight_checks.c:68
preflight_checks.h
preflight_check_t
Definition: preflight_checks.h:43
preflight_result_t
Definition: preflight_checks.h:33
state.h
API to get/set the generic vehicle states.
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78