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