Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
•All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
state.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2011-2012 Felix Ruess <felix.ruess@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
34#include "state.h"
35
36struct State state;
37
38#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
39/* Preflight checks */
40#include "modules/checks/preflight_checks.h"
41static struct preflight_check_t state_pfc;
42
43static 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
57void 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
85void 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 ******************************************************************************/
124struct 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
143struct 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
160struct 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
175struct 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
190int32_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
204float 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
217void 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
249void 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
329void 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
416void 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
514void 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
551void 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
582void 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
649void 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
722void 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
823void 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
873void 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
925void 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
950void 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
1026
1027void 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
1077void 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
1127void 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
1178
1209/******************************************************************************
1210 * *
1211 * Transformation functions for the ACCELERATION representations *
1212 * *
1213 *****************************************************************************/
1217void 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
1250void 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
1282void 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
1315void 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 *****************************************************************************/
1356void 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
1371void 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 *****************************************************************************/
1397void 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
1413void 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
1428void 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
1443void 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
1459void 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
1474void 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
foo
uint16_t foo
Definition main_demo5.c:58
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
lla_of_utm_f
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
Definition pprz_geodetic_float.c:344
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