Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
state.h
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
30
31#ifndef STATE_H
32#define STATE_H
33
34#ifdef __cplusplus
35extern "C" {
36#endif
37
43
44#include "std.h"
45#include <string.h>
46
61
66
71#define POS_ECEF_I 0
72#define POS_NED_I 1
73#define POS_ENU_I 2
74#define POS_LLA_I 3
75#define POS_UTM_I 4
76#define POS_ECEF_F 5
77#define POS_NED_F 6
78#define POS_ENU_F 7
79#define POS_LLA_F 8
80#define POS_UTM_F 9
81#define POS_LOCAL_COORD ((1<<POS_NED_I)|(1<<POS_NED_F)|(1<<POS_ENU_I)|(1<<POS_ENU_F))
82#define POS_GLOBAL_COORD ((1<<POS_ECEF_I)|(1<<POS_ECEF_F)|(1<<POS_LLA_I)|(1<<POS_LLA_F)|(1<<POS_UTM_I)|(1<<POS_UTM_F))
84
89#define SPEED_ECEF_I 0
90#define SPEED_NED_I 1
91#define SPEED_ENU_I 2
92#define SPEED_HNORM_I 3
93#define SPEED_HDIR_I 4
94#define SPEED_ECEF_F 5
95#define SPEED_NED_F 6
96#define SPEED_ENU_F 7
97#define SPEED_HNORM_F 8
98#define SPEED_HDIR_F 9
99#define SPEED_LOCAL_COORD ((1<<SPEED_NED_I)|(1<<SPEED_ENU_I)|(1<<SPEED_NED_F)|(1<<SPEED_ENU_F))
101
106#define ACCEL_ECEF_I 0
107#define ACCEL_NED_I 1
108#define ACCEL_ECEF_F 2
109#define ACCEL_NED_F 3
111
116#define RATE_I 0
117#define RATE_F 1
119
124#define WINDSPEED_I 0
125#define DOWNWIND_I 1
126#define AIRSPEED_I 2
127#define WINDSPEED_F 3
128#define DOWNWIND_F 4
129#define AIRSPEED_F 5
130#define AOA_F 6
131#define SIDESLIP_F 7
133
138#define STATE_INPUT_DISABLE 0
139#define STATE_INPUT_ANY 0xFFFF
140#define STATE_INPUT_ORIGIN 1
141#define STATE_INPUT_POS 2
142#define STATE_INPUT_SPEED 3
143#define STATE_INPUT_ACCEL 4
144#define STATE_INPUT_ATTITUDE 5
145#define STATE_INPUT_RATES 6
146#define STATE_INPUT_WIND_AIR 7
148
152struct State {
153
156
161
168
173
179
186
195
200
207
214
221
227
234
240
249
252
262
265
272
280
281
290
295
301
307
313
319
326
332
337
343
349
356
357
366
371
377
383
389
395
402
403
407
412
413
422
427
433
440
441
450
455
464
470
479
485
491
497
499
502
508
510
511};
512
513extern struct State state;
514
515extern void stateInit(void);
516
521extern void stateSetInputFilter(uint8_t type, uint16_t id);
522
524#define STATE_CHECK_INPUT_AND_RETURN(flag, id) if (flag==STATE_INPUT_DISABLE || !(flag==STATE_INPUT_ANY || flag==id)) { return; }
525
528
530static inline void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
531{
532 STATE_CHECK_INPUT_AND_RETURN(state.origin_input_filter, id);
533
534 state.ned_origin_i = *ltp_def;
535 /* convert to float */
536 ECEF_FLOAT_OF_BFP(state.ned_origin_f.ecef, state.ned_origin_i.ecef);
537 LLA_FLOAT_OF_BFP(state.ned_origin_f.lla, state.ned_origin_i.lla);
538 HIGH_RES_RMAT_FLOAT_OF_BFP(state.ned_origin_f.ltp_of_ecef, state.ned_origin_i.ltp_of_ecef);
539 state.ned_origin_f.hmsl = M_OF_MM(state.ned_origin_i.hmsl);
540
541 /* clear bits for all local frame representations */
542 state.pos_status &= ~(POS_LOCAL_COORD);
543 state.speed_status &= ~(SPEED_LOCAL_COORD);
544 ClearBit(state.accel_status, ACCEL_NED_I);
545 ClearBit(state.accel_status, ACCEL_NED_F);
546
547 state.ned_initialized_i = true;
548 state.ned_initialized_f = true;
549}
550
552static inline void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
553{
554 STATE_CHECK_INPUT_AND_RETURN(state.origin_input_filter, id);
555
556 state.utm_origin_f = *utm_def;
557 state.utm_initialized_f = true;
558
559 /* clear bits for all local frame representations */
560 state.pos_status &= ~(POS_LOCAL_COORD);
561 state.speed_status &= ~(SPEED_LOCAL_COORD);
562 ClearBit(state.accel_status, ACCEL_NED_I);
563 ClearBit(state.accel_status, ACCEL_NED_F);
564}
565
567static inline struct LtpDef_i *stateGetNedOrigin_i(void)
568{
569 if (state.ned_initialized_i) {
570 return &state.ned_origin_i;
571 } else {
572 return NULL;
573 }
574}
575
577static inline struct LtpDef_f *stateGetNedOrigin_f(void)
578{
579 if (state.ned_initialized_f) {
580 return &state.ned_origin_f;
581 } else {
582 return NULL;
583 }
584}
585
587static inline struct UtmCoor_f *stateGetUtmOrigin_f(void)
588{
589 if (state.utm_initialized_f) {
590 return &state.utm_origin_f;
591 } else {
592 return NULL;
593 }
594}
595
596/************* declaration of origin accessor function ************/
597extern struct LlaCoor_i stateGetLlaOrigin_i(void);
598extern struct LlaCoor_f stateGetLlaOrigin_f(void);
599extern struct EcefCoor_i stateGetEcefOrigin_i(void);
600extern struct EcefCoor_f stateGetEcefOrigin_f(void);
601extern int32_t stateGetHmslOrigin_i(void);
602extern float stateGetHmslOrigin_f(void);
603
604/*******************************************************************************
605 * *
606 * Set and Get functions for the POSITION representations *
607 * *
608 ******************************************************************************/
609
610/************* declaration of transformation functions ************/
611extern void stateCalcPositionEcef_i(void);
612extern void stateCalcPositionNed_i(void);
613extern void stateCalcPositionEnu_i(void);
614extern void stateCalcPositionLla_i(void);
615extern void stateCalcPositionUtm_f(void);
616extern void stateCalcPositionEcef_f(void);
617extern void stateCalcPositionNed_f(void);
618extern void stateCalcPositionEnu_f(void);
619extern void stateCalcPositionLla_f(void);
620
621/*********************** validity test functions ******************/
622
624static inline bool stateIsLocalCoordinateValid(void)
625{
626 return ((state.ned_initialized_i || state.ned_initialized_f || state.utm_initialized_f)
627 && (state.pos_status & (POS_LOCAL_COORD)));
628}
629
631static inline bool stateIsGlobalCoordinateValid(void)
632{
633 return ((state.pos_status & (POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid());
634}
635
636/************************ Set functions ****************************/
637
639static inline void stateSetPositionEcef_i(uint16_t id, struct EcefCoor_i *ecef_pos)
640{
641 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
642
643 VECT3_COPY(state.ecef_pos_i, *ecef_pos);
644 /* clear bits for all position representations and only set the new one */
645 state.pos_status = (1 << POS_ECEF_I);
646}
647
649static inline void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
650{
651 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
652
653 VECT3_COPY(state.ned_pos_i, *ned_pos);
654 /* clear bits for all position representations and only set the new one */
655 state.pos_status = (1 << POS_NED_I);
656}
657
659static inline void stateSetPositionEnu_i(uint16_t id, struct EnuCoor_i *enu_pos)
660{
661 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
662
663 VECT3_COPY(state.enu_pos_i, *enu_pos);
664 /* clear bits for all position representations and only set the new one */
665 state.pos_status = (1 << POS_ENU_I);
666}
667
669static inline void stateSetPositionLla_i(uint16_t id, struct LlaCoor_i *lla_pos)
670{
671 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
672
673 LLA_COPY(state.lla_pos_i, *lla_pos);
674 /* clear bits for all position representations and only set the new one */
675 state.pos_status = (1 << POS_LLA_I);
676}
677
679static inline void stateSetPosition_i(
680 uint16_t id,
681 struct EcefCoor_i *ecef_pos,
682 struct NedCoor_i *ned_pos,
683 struct EnuCoor_i *enu_pos,
684 struct LlaCoor_i *lla_pos)
685{
686 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
687
688 /* clear all status bit */
689 state.pos_status = 0;
690 if (ecef_pos != NULL) {
691 VECT3_COPY(state.ecef_pos_i, *ecef_pos);
692 state.pos_status |= (1 << POS_ECEF_I);
693 }
694 if (ned_pos != NULL) {
695 VECT3_COPY(state.ned_pos_i, *ned_pos);
696 state.pos_status |= (1 << POS_NED_I);
697 }
698 if (enu_pos != NULL) {
699 VECT3_COPY(state.enu_pos_i, *enu_pos);
700 state.pos_status |= (1 << POS_ENU_I);
701 }
702 if (lla_pos != NULL) {
703 LLA_COPY(state.lla_pos_i, *lla_pos);
704 state.pos_status |= (1 << POS_LLA_I);
705 }
706}
707
709static inline void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
710{
711 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
712
713 state.utm_pos_f = *utm_pos;
714 /* clear bits for all position representations and only set the new one */
715 state.pos_status = (1 << POS_UTM_F);
716}
717
719static inline void stateSetPositionEcef_f(uint16_t id, struct EcefCoor_f *ecef_pos)
720{
721 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
722
723 VECT3_COPY(state.ecef_pos_f, *ecef_pos);
724 /* clear bits for all position representations and only set the new one */
725 state.pos_status = (1 << POS_ECEF_F);
726}
727
729static inline void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
730{
731 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
732
733 VECT3_COPY(state.ned_pos_f, *ned_pos);
734 /* clear bits for all position representations and only set the new one */
735 state.pos_status = (1 << POS_NED_F);
736}
737
739static inline void stateSetPositionEnu_f(uint16_t id, struct EnuCoor_f *enu_pos)
740{
741 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
742
743 VECT3_COPY(state.enu_pos_f, *enu_pos);
744 /* clear bits for all position representations and only set the new one */
745 state.pos_status = (1 << POS_ENU_F);
746}
747
749static inline void stateSetPositionLla_f(uint16_t id, struct LlaCoor_f *lla_pos)
750{
751 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
752
753 LLA_COPY(state.lla_pos_f, *lla_pos);
754 /* clear bits for all position representations and only set the new one */
755 state.pos_status = (1 << POS_LLA_F);
756}
757
759static inline void stateSetPosition_f(
760 uint16_t id,
761 struct EcefCoor_f *ecef_pos,
762 struct NedCoor_f *ned_pos,
763 struct EnuCoor_f *enu_pos,
764 struct LlaCoor_f *lla_pos,
765 struct UtmCoor_f *utm_pos)
766{
767 STATE_CHECK_INPUT_AND_RETURN(state.pos_input_filter, id);
768
769 /* clear all status bit */
770 state.pos_status = 0;
771 if (ecef_pos != NULL) {
772 VECT3_COPY(state.ecef_pos_f, *ecef_pos);
773 state.pos_status |= (1 << POS_ECEF_F);
774 }
775 if (ned_pos != NULL) {
776 VECT3_COPY(state.ned_pos_f, *ned_pos);
777 state.pos_status |= (1 << POS_NED_F);
778 }
779 if (enu_pos != NULL) {
780 VECT3_COPY(state.enu_pos_f, *enu_pos);
781 state.pos_status |= (1 << POS_ENU_F);
782 }
783 if (lla_pos != NULL) {
784 LLA_COPY(state.lla_pos_f, *lla_pos);
785 state.pos_status |= (1 << POS_LLA_F);
786 }
787 if (utm_pos != NULL) {
788 state.utm_pos_f = *utm_pos;
789 state.pos_status |= (1 << POS_UTM_F);
790 }
791}
792
793/************************ Get functions ****************************/
794
796static inline struct EcefCoor_i *stateGetPositionEcef_i(void)
797{
798 if (!bit_is_set(state.pos_status, POS_ECEF_I)) {
800 }
801 return &state.ecef_pos_i;
802}
803
805static inline struct NedCoor_i *stateGetPositionNed_i(void)
806{
807 if (!bit_is_set(state.pos_status, POS_NED_I)) {
809 }
810 return &state.ned_pos_i;
811}
812
814static inline struct EnuCoor_i *stateGetPositionEnu_i(void)
815{
816 if (!bit_is_set(state.pos_status, POS_ENU_I)) {
818 }
819 return &state.enu_pos_i;
820}
821
823static inline struct LlaCoor_i *stateGetPositionLla_i(void)
824{
825 if (!bit_is_set(state.pos_status, POS_LLA_I)) {
827 }
828 return &state.lla_pos_i;
829}
830
832static inline struct UtmCoor_f *stateGetPositionUtm_f(void)
833{
834 if (!bit_is_set(state.pos_status, POS_UTM_F)) {
836 }
837 return &state.utm_pos_f;
838}
839
841static inline struct EcefCoor_f *stateGetPositionEcef_f(void)
842{
843 if (!bit_is_set(state.pos_status, POS_ECEF_F)) {
845 }
846 return &state.ecef_pos_f;
847}
848
850static inline struct NedCoor_f *stateGetPositionNed_f(void)
851{
852 if (!bit_is_set(state.pos_status, POS_NED_F)) {
854 }
855 return &state.ned_pos_f;
856}
857
859static inline struct EnuCoor_f *stateGetPositionEnu_f(void)
860{
861 if (!bit_is_set(state.pos_status, POS_ENU_F)) {
863 }
864 return &state.enu_pos_f;
865}
866
868static inline struct LlaCoor_f *stateGetPositionLla_f(void)
869{
870 if (!bit_is_set(state.pos_status, POS_LLA_F)) {
872 }
873 return &state.lla_pos_f;
874}
875
877
878
879
880/******************************************************************************
881 * *
882 * Set and Get functions for the SPEED representations *
883 * *
884 *****************************************************************************/
887
888/************* declaration of transformation functions ************/
889extern void stateCalcSpeedNed_i(void);
890extern void stateCalcSpeedEnu_i(void);
891extern void stateCalcSpeedEcef_i(void);
892extern void stateCalcHorizontalSpeedNorm_i(void);
893extern void stateCalcHorizontalSpeedDir_i(void);
894extern void stateCalcSpeedNed_f(void);
895extern void stateCalcSpeedEnu_f(void);
896extern void stateCalcSpeedEcef_f(void);
897extern void stateCalcHorizontalSpeedNorm_f(void);
898extern void stateCalcHorizontalSpeedDir_f(void);
899
900/************************ Set functions ****************************/
901
903static inline void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
904{
905 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
906
907 VECT3_COPY(state.ned_speed_i, *ned_speed);
908 /* clear bits for all speed representations and only set the new one */
909 state.speed_status = (1 << SPEED_NED_I);
910}
911
913static inline void stateSetSpeedEnu_i(uint16_t id, struct EnuCoor_i *enu_speed)
914{
915 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
916
917 VECT3_COPY(state.enu_speed_i, *enu_speed);
918 /* clear bits for all speed representations and only set the new one */
919 state.speed_status = (1 << SPEED_ENU_I);
920}
921
923static inline void stateSetSpeedEcef_i(uint16_t id, struct EcefCoor_i *ecef_speed)
924{
925 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
926
927 VECT3_COPY(state.ecef_speed_i, *ecef_speed);
928 /* clear bits for all speed representations and only set the new one */
929 state.speed_status = (1 << SPEED_ECEF_I);
930}
931
933static inline void stateSetSpeed_i(
934 uint16_t id,
935 struct EcefCoor_i *ecef_speed,
936 struct NedCoor_i *ned_speed,
937 struct EnuCoor_i *enu_speed)
938{
939 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
940
941 /* clear all status bit */
942 state.speed_status = 0;
943 if (ecef_speed != NULL) {
944 VECT3_COPY(state.ecef_speed_i, *ecef_speed);
945 state.speed_status |= (1 << SPEED_ECEF_I);
946 }
947 if (ned_speed != NULL) {
948 VECT3_COPY(state.ned_speed_i, *ned_speed);
949 state.speed_status |= (1 << SPEED_NED_I);
950 }
951 if (enu_speed != NULL) {
952 VECT3_COPY(state.enu_speed_i, *enu_speed);
953 state.speed_status |= (1 << SPEED_ENU_I);
954 }
955}
956
958static inline void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
959{
960 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
961
962 VECT3_COPY(state.ned_speed_f, *ned_speed);
963 /* clear bits for all speed representations and only set the new one */
964 state.speed_status = (1 << SPEED_NED_F);
965}
966
968static inline void stateSetSpeedEnu_f(uint16_t id, struct EnuCoor_f *enu_speed)
969{
970 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
971
972 VECT3_COPY(state.enu_speed_f, *enu_speed);
973 /* clear bits for all speed representations and only set the new one */
974 state.speed_status = (1 << SPEED_ENU_F);
975}
976
978static inline void stateSetSpeedEcef_f(uint16_t id, struct EcefCoor_f *ecef_speed)
979{
980 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
981
982 VECT3_COPY(state.ecef_speed_f, *ecef_speed);
983 /* clear bits for all speed representations and only set the new one */
984 state.speed_status = (1 << SPEED_ECEF_F);
985}
986
988static inline void stateSetSpeed_f(
989 uint16_t id,
990 struct EcefCoor_f *ecef_speed,
991 struct NedCoor_f *ned_speed,
992 struct EnuCoor_f *enu_speed)
993{
994 STATE_CHECK_INPUT_AND_RETURN(state.speed_input_filter, id);
995
996 /* clear all status bit */
997 state.speed_status = 0;
998 if (ecef_speed != NULL) {
999 VECT3_COPY(state.ecef_speed_f, *ecef_speed);
1000 state.speed_status |= (1 << SPEED_ECEF_F);
1001 }
1002 if (ned_speed != NULL) {
1003 VECT3_COPY(state.ned_speed_f, *ned_speed);
1004 state.speed_status |= (1 << SPEED_NED_F);
1005 }
1006 if (enu_speed != NULL) {
1007 VECT3_COPY(state.enu_speed_f, *enu_speed);
1008 state.speed_status |= (1 << SPEED_ENU_F);
1009 }
1010}
1011
1012/************************ Get functions ****************************/
1013
1015static inline struct NedCoor_i *stateGetSpeedNed_i(void)
1016{
1017 if (!bit_is_set(state.speed_status, SPEED_NED_I)) {
1019 }
1020 return &state.ned_speed_i;
1021}
1022
1024static inline struct EnuCoor_i *stateGetSpeedEnu_i(void)
1025{
1026 if (!bit_is_set(state.speed_status, SPEED_ENU_I)) {
1028 }
1029 return &state.enu_speed_i;
1030}
1031
1033static inline struct EcefCoor_i *stateGetSpeedEcef_i(void)
1034{
1035 if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) {
1037 }
1038 return &state.ecef_speed_i;
1039}
1040
1043{
1044 if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) {
1046 }
1047 return state.h_speed_norm_i;
1048}
1049
1052{
1053 if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) {
1055 }
1056 return state.h_speed_dir_i;
1057}
1058
1060static inline struct NedCoor_f *stateGetSpeedNed_f(void)
1061{
1062 if (!bit_is_set(state.speed_status, SPEED_NED_F)) {
1064 }
1065 return &state.ned_speed_f;
1066}
1067
1069static inline struct EnuCoor_f *stateGetSpeedEnu_f(void)
1070{
1071 if (!bit_is_set(state.speed_status, SPEED_ENU_F)) {
1073 }
1074 return &state.enu_speed_f;
1075}
1076
1078static inline struct EcefCoor_f *stateGetSpeedEcef_f(void)
1079{
1080 if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) {
1082 }
1083 return &state.ecef_speed_f;
1084}
1085
1087static inline float stateGetHorizontalSpeedNorm_f(void)
1088{
1089 if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) {
1091 }
1092 return state.h_speed_norm_f;
1093}
1094
1096static inline float stateGetHorizontalSpeedDir_f(void)
1097{
1098 if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) {
1100 }
1101 return state.h_speed_dir_f;
1102}
1103
1105static inline struct Int32Vect3 *stateGetAccelBody_i(void)
1106{
1107 return &state.body_accel_i;
1108}
1109
1110
1111static inline struct FloatVect3 *stateGetAccelBody_f(void)
1112{
1113 static struct FloatVect3 body_accel_f;
1114 body_accel_f.x = FLOAT_OF_BFP(state.body_accel_i.x, INT32_ACCEL_FRAC);
1115 body_accel_f.y = FLOAT_OF_BFP(state.body_accel_i.y, INT32_ACCEL_FRAC);
1116 body_accel_f.z = FLOAT_OF_BFP(state.body_accel_i.z, INT32_ACCEL_FRAC);
1117 return &body_accel_f;
1118}
1119
1120
1121/******************************************************************************
1122 * *
1123 * Set and Get functions for the ACCELERATION representations *
1124 * *
1125 *****************************************************************************/
1128
1129/************* declaration of transformation functions ************/
1130extern void stateCalcAccelNed_i(void);
1131extern void stateCalcAccelEcef_i(void);
1132extern void stateCalcAccelNed_f(void);
1133extern void stateCalcAccelEcef_f(void);
1134
1135/*********************** validity test functions ******************/
1136
1138static inline bool stateIsAccelValid(void)
1139{
1140 return (state.accel_status);
1141}
1142
1143/************************ Set functions ****************************/
1144
1146static inline void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
1147{
1148 STATE_CHECK_INPUT_AND_RETURN(state.accel_input_filter, id);
1149
1150 VECT3_COPY(state.ned_accel_i, *ned_accel);
1151 /* clear bits for all accel representations and only set the new one */
1152 state.accel_status = (1 << ACCEL_NED_I);
1153}
1154
1156static inline void stateSetAccelEcef_i(uint16_t id, struct EcefCoor_i *ecef_accel)
1157{
1158 STATE_CHECK_INPUT_AND_RETURN(state.accel_input_filter, id);
1159
1160 VECT3_COPY(state.ecef_accel_i, *ecef_accel);
1161 /* clear bits for all accel representations and only set the new one */
1162 state.accel_status = (1 << ACCEL_ECEF_I);
1163}
1164
1166static inline void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
1167{
1168 STATE_CHECK_INPUT_AND_RETURN(state.accel_input_filter, id);
1169
1170 VECT3_COPY(state.ned_accel_f, *ned_accel);
1171 /* clear bits for all accel representations and only set the new one */
1172 state.accel_status = (1 << ACCEL_NED_F);
1173}
1174
1176static inline void stateSetAccelEcef_f(uint16_t id, struct EcefCoor_f *ecef_accel)
1177{
1178 STATE_CHECK_INPUT_AND_RETURN(state.accel_input_filter, id);
1179
1180 VECT3_COPY(state.ecef_accel_f, *ecef_accel);
1181 /* clear bits for all accel representations and only set the new one */
1182 state.accel_status = (1 << ACCEL_ECEF_F);
1183}
1184
1186static inline void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
1187{
1188 STATE_CHECK_INPUT_AND_RETURN(state.accel_input_filter, id);
1189
1190 VECT3_COPY(state.body_accel_i, *body_accel);
1191}
1192
1193/************************ Get functions ****************************/
1194
1196static inline struct NedCoor_i *stateGetAccelNed_i(void)
1197{
1198 if (!bit_is_set(state.accel_status, ACCEL_NED_I)) {
1200 }
1201 return &state.ned_accel_i;
1202}
1203
1205static inline struct EcefCoor_i *stateGetAccelEcef_i(void)
1206{
1207 if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1209 }
1210 return &state.ecef_accel_i;
1211}
1212
1214static inline struct NedCoor_f *stateGetAccelNed_f(void)
1215{
1216 if (!bit_is_set(state.accel_status, ACCEL_NED_F)) {
1218 }
1219 return &state.ned_accel_f;
1220}
1221
1223static inline struct EcefCoor_f *stateGetAccelEcef_f(void)
1224{
1225 if (!bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1227 }
1228 return &state.ecef_accel_f;
1229}
1230
1231
1232/******************************************************************************
1233* *
1234* Set and Get functions for the ATTITUDE representations *
1235* (Calls the functions in math/pprz_orientation_conversion) *
1236* *
1237*****************************************************************************/
1240/*********************** validity test functions ******************/
1241
1243static inline bool stateIsAttitudeValid(void)
1244{
1245 return (orienationCheckValid(&state.ned_to_body_orientation));
1246}
1247
1248/************************ Set functions ****************************/
1249
1251static inline void stateSetNedToBodyQuat_i(uint16_t id, struct Int32Quat *ned_to_body_quat)
1252{
1253 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1254 orientationSetQuat_i(&state.ned_to_body_orientation, ned_to_body_quat);
1255}
1256
1258static inline void stateSetNedToBodyRMat_i(uint16_t id, struct Int32RMat *ned_to_body_rmat)
1259{
1260 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1261 orientationSetRMat_i(&state.ned_to_body_orientation, ned_to_body_rmat);
1262}
1263
1265static inline void stateSetNedToBodyEulers_i(uint16_t id, struct Int32Eulers *ned_to_body_eulers)
1266{
1267 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1268 orientationSetEulers_i(&state.ned_to_body_orientation, ned_to_body_eulers);
1269}
1270
1272static inline void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
1273{
1274 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1275 orientationSetQuat_f(&state.ned_to_body_orientation, ned_to_body_quat);
1276}
1277
1279static inline void stateSetNedToBodyRMat_f(uint16_t id, struct FloatRMat *ned_to_body_rmat)
1280{
1281 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1282 orientationSetRMat_f(&state.ned_to_body_orientation, ned_to_body_rmat);
1283}
1284
1286static inline void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
1287{
1288 STATE_CHECK_INPUT_AND_RETURN(state.attitude_input_filter, id);
1289 orientationSetEulers_f(&state.ned_to_body_orientation, ned_to_body_eulers);
1290}
1291
1292/************************ Get functions ****************************/
1293
1295static inline struct Int32Quat *stateGetNedToBodyQuat_i(void)
1296{
1297 return orientationGetQuat_i(&state.ned_to_body_orientation);
1298}
1299
1301static inline struct Int32RMat *stateGetNedToBodyRMat_i(void)
1302{
1303 return orientationGetRMat_i(&state.ned_to_body_orientation);
1304}
1305
1307static inline struct Int32Eulers *stateGetNedToBodyEulers_i(void)
1308{
1309 return orientationGetEulers_i(&state.ned_to_body_orientation);
1310}
1311
1313static inline struct FloatQuat *stateGetNedToBodyQuat_f(void)
1314{
1315 return orientationGetQuat_f(&state.ned_to_body_orientation);
1316}
1317
1319static inline struct FloatRMat *stateGetNedToBodyRMat_f(void)
1320{
1321 return orientationGetRMat_f(&state.ned_to_body_orientation);
1322}
1323
1325static inline struct FloatEulers *stateGetNedToBodyEulers_f(void)
1326{
1327 return orientationGetEulers_f(&state.ned_to_body_orientation);
1328}
1329
1332{
1333 return orientationGetEulersZxy_f(&state.ned_to_body_orientation);
1334}
1335
1336
1337
1338/******************************************************************************
1339 * *
1340 * Set and Get functions for the ANGULAR RATE representations *
1341 * *
1342 *****************************************************************************/
1345
1346/************* declaration of transformation functions ************/
1347extern void stateCalcBodyRates_i(void);
1348extern void stateCalcBodyRates_f(void);
1349
1350/*********************** validity test functions ******************/
1351
1353static inline bool stateIsRateValid(void)
1354{
1355 return (state.rate_status);
1356}
1357
1358/************************ Set functions ****************************/
1359
1361static inline void stateSetBodyRates_i(uint16_t id, struct Int32Rates *body_rate)
1362{
1363 STATE_CHECK_INPUT_AND_RETURN(state.rates_input_filter, id);
1364
1365 RATES_COPY(state.body_rates_i, *body_rate);
1366 /* clear bits for all attitude representations and only set the new one */
1367 state.rate_status = (1 << RATE_I);
1368}
1369
1371static inline void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
1372{
1373 STATE_CHECK_INPUT_AND_RETURN(state.rates_input_filter, id);
1374
1375 RATES_COPY(state.body_rates_f, *body_rate);
1376 /* clear bits for all attitude representations and only set the new one */
1377 state.rate_status = (1 << RATE_F);
1378}
1379
1380/************************ Get functions ****************************/
1381
1383static inline struct Int32Rates *stateGetBodyRates_i(void)
1384{
1385 if (!bit_is_set(state.rate_status, RATE_I)) {
1387 }
1388 return &state.body_rates_i;
1389}
1390
1392static inline struct FloatRates *stateGetBodyRates_f(void)
1393{
1394 if (!bit_is_set(state.rate_status, RATE_F)) {
1396 }
1397 return &state.body_rates_f;
1398}
1399
1401
1402
1403
1404/******************************************************************************
1405 * *
1406 * Set and Get functions for the WIND- AND AIRSPEED representations *
1407 * *
1408 *****************************************************************************/
1411
1412/************* declaration of transformation functions ************/
1413extern void stateCalcHorizontalWindspeed_i(void);
1414extern void stateCalcVerticalWindspeed_i(void);
1415extern void stateCalcAirspeed_i(void);
1416extern void stateCalcHorizontalWindspeed_f(void);
1417extern void stateCalcVerticalWindspeed_f(void);
1418extern void stateCalcAirspeed_f(void);
1419
1420
1421/************************ validity test function *******************/
1422
1424static inline bool stateIsWindspeedValid(void)
1425{
1426 return (state.wind_air_status & ((1 << WINDSPEED_I) | (1 << WINDSPEED_F)));
1427}
1428
1430static inline bool stateIsVerticalWindspeedValid(void)
1431{
1432 return (state.wind_air_status & ((1 << DOWNWIND_I) | (1 << DOWNWIND_F)));
1433}
1434
1436static inline bool stateIsAirspeedValid(void)
1437{
1438 return (state.wind_air_status & ((1 << AIRSPEED_I) | (1 << AIRSPEED_F)));
1439}
1440
1442static inline bool stateIsAngleOfAttackValid(void)
1443{
1444 return (state.wind_air_status & (1 << AOA_F));
1445}
1446
1448static inline bool stateIsSideslipValid(void)
1449{
1450 return (state.wind_air_status & (1 << SIDESLIP_F));
1451}
1452
1453/************************ Set functions ****************************/
1454
1456static inline void stateSetHorizontalWindspeed_i(uint16_t id, struct Int32Vect2 *h_windspeed)
1457{
1458 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1459
1460 VECT2_COPY(state.windspeed_i.vect2, *h_windspeed);
1461 /* clear bits for all horizontal windspeed representations and only set the new one */
1462 ClearBit(state.wind_air_status, WINDSPEED_F);
1463 SetBit(state.wind_air_status, WINDSPEED_I);
1464}
1465
1467static inline void stateSetVerticalWindspeed_i(uint16_t id, int32_t v_windspeed)
1468{
1469 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1470
1471 state.windspeed_i.vect3.z = v_windspeed;
1472 /* clear bits for all vertical windspeed representations and only set the new one */
1473 ClearBit(state.wind_air_status, DOWNWIND_F);
1474 SetBit(state.wind_air_status, DOWNWIND_I);
1475}
1476
1478static inline void stateSetAirspeed_i(uint16_t id, int32_t airspeed)
1479{
1480 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1481
1482 state.airspeed_i = airspeed;
1483 /* clear bits for all airspeed representations and only set the new one */
1484 ClearBit(state.wind_air_status, AIRSPEED_F);
1485 SetBit(state.wind_air_status, AIRSPEED_I);
1486}
1487
1489static inline void stateSetHorizontalWindspeed_f(uint16_t id, struct FloatVect2 *h_windspeed)
1490{
1491 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1492
1493 VECT2_COPY(state.windspeed_f.vect2, *h_windspeed);
1494 /* clear bits for all horizontal windspeed representations and only set the new one */
1495 ClearBit(state.wind_air_status, WINDSPEED_I);
1496 SetBit(state.wind_air_status, WINDSPEED_F);
1497}
1498
1500static inline void stateSetVerticalWindspeed_f(uint16_t id, float v_windspeed)
1501{
1502 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1503
1504 state.windspeed_f.vect3.z = v_windspeed;
1505 /* clear bits for all vertical windspeed representations and only set the new one */
1506 ClearBit(state.wind_air_status, DOWNWIND_I);
1507 SetBit(state.wind_air_status, DOWNWIND_F);
1508}
1509
1511static inline void stateSetAirspeed_f(uint16_t id, float airspeed)
1512{
1513 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1514
1515 state.airspeed_f = airspeed;
1516 /* clear bits for all airspeed representations and only set the new one */
1517 ClearBit(state.wind_air_status, AIRSPEED_I);
1518 SetBit(state.wind_air_status, AIRSPEED_F);
1519}
1520
1522static inline void stateSetAngleOfAttack_f(uint16_t id, float aoa)
1523{
1524 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1525
1526 state.angle_of_attack_f = aoa;
1527 /* clear bits for all AOA representations and only set the new one */
1529 SetBit(state.wind_air_status, AOA_F);
1530}
1531
1533static inline void stateSetSideslip_f(uint16_t id, float sideslip)
1534{
1535 STATE_CHECK_INPUT_AND_RETURN(state.wind_air_input_filter, id);
1536
1537 state.sideslip_f = sideslip;
1538 /* clear bits for all sideslip representations and only set the new one */
1540 SetBit(state.wind_air_status, SIDESLIP_F);
1541}
1542
1543/************************ Get functions ****************************/
1544
1547{
1548 if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1550 }
1551 return &state.windspeed_i.vect2;
1552}
1553
1555static inline float stateGetVerticalWindspeed_i(void)
1556{
1557 if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1559 }
1560 return state.windspeed_i.vect3.z;
1561}
1562
1564static inline struct Int32Vect3 *stateGetWindspeed_i(void)
1565{
1566 if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1568 }
1569 if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1571 }
1572 return &state.windspeed_i.vect3;
1573}
1574
1576static inline int32_t stateGetAirspeed_i(void)
1577{
1578 if (!bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1580 }
1581 return state.airspeed_i;
1582}
1583
1586{
1587 if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1589 }
1590 return &state.windspeed_f.vect2;
1591}
1592
1594static inline float stateGetVerticalWindspeed_f(void)
1595{
1596 if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1598 }
1599 return state.windspeed_f.vect3.z;
1600}
1601
1603static inline struct FloatVect3 *stateGetWindspeed_f(void)
1604{
1605 if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1607 }
1608 if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1610 }
1611 return &state.windspeed_f.vect3;
1612}
1613
1615static inline float stateGetAirspeed_f(void)
1616{
1617 if (!bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1619 }
1620 return state.airspeed_f;
1621}
1622
1624static inline float stateGetAngleOfAttack_f(void)
1625{
1627// if (!bit_is_set(state.wind_air_status, AOA_F))
1628// stateCalcAOA_f();
1629 return state.angle_of_attack_f;
1630}
1631
1633static inline float stateGetSideslip_f(void)
1634{
1636// if (!bit_is_set(state.wind_air_status, SIDESLIP_F))
1637// stateCalcSideslip_f();
1638 return state.sideslip_f;
1639}
1640
1642
1645
1647static inline void stateSetFixedWingFlight(bool fixed_wing)
1648{
1649 state.fixed_wing_flight = fixed_wing;
1650}
1651
1653static inline bool stateIsFixedWingFlight(void)
1654{
1655 return state.fixed_wing_flight;
1656}
1657
1659
1660#ifdef __cplusplus
1661} /* extern "C" */
1662#endif
1663
1664
1665#endif /* STATE_H */
euler angles
Roation quaternion.
rotation matrix
angular rates
#define RATES_COPY(_a, _b)
#define VECT2_COPY(_a, _b)
#define VECT3_COPY(_a, _b)
#define INT32_ACCEL_FRAC
#define FLOAT_OF_BFP(_vbfp, _frac)
euler angles
Rotation quaternion.
rotation matrix
angular rates
#define LLA_COPY(_pos1, _pos2)
#define ECEF_FLOAT_OF_BFP(_o, _i)
#define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei)
#define LLA_FLOAT_OF_BFP(_o, _i)
#define M_OF_MM(_mm)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
static void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat)
Set vehicle body attitude from rotation matrix (float).
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
static bool orienationCheckValid(struct OrientationReps *orientation)
Test if orientations are valid.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static struct FloatEulers * orientationGetEulersZxy_f(struct OrientationReps *orientation)
Get orientation as ZXY euler angles (float).
static void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
Set vehicle body attitude from quaternion (int).
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
static void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
Set vehicle body attitude from rotation matrix (int).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
#define ACCEL_ECEF_I
Definition state.h:106
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition state.h:1146
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition state.h:400
struct Int32Vect3 body_accel_i
Acceleration in Body coordinates.
Definition state.h:376
static struct EcefCoor_i * stateGetAccelEcef_i(void)
Get acceleration in ECEF coordinates (int).
Definition state.h:1205
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition state.h:382
#define ACCEL_NED_F
Definition state.h:109
static struct EcefCoor_f * stateGetAccelEcef_f(void)
Get acceleration in ECEF coordinates (float).
Definition state.h:1223
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition state.h:365
void stateCalcAccelNed_i(void)
Definition state.c:1223
void stateCalcAccelEcef_i(void)
Definition state.c:1256
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition state.h:388
static void stateSetAccelEcef_f(uint16_t id, struct EcefCoor_f *ecef_accel)
Set acceleration in ECEF coordinates (float).
Definition state.h:1176
static bool stateIsAccelValid(void)
Test if accelerations are valid.
Definition state.h:1138
#define ACCEL_ECEF_F
Definition state.h:108
void stateCalcAccelEcef_f(void)
Definition state.c:1321
uint16_t accel_input_filter
Holds the input filter id for acceleration.
Definition state.h:370
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1166
static void stateSetAccelEcef_i(uint16_t id, struct EcefCoor_i *ecef_accel)
Set acceleration in ECEF coordinates (int).
Definition state.h:1156
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition state.h:394
#define ACCEL_NED_I
Definition state.h:107
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
void stateCalcAccelNed_f(void)
Definition state.c:1288
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition state.h:1214
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition state.h:1196
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1319
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
static void stateSetNedToBodyQuat_i(uint16_t id, struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition state.h:1251
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition state.h:1243
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition state.h:1286
static void stateSetNedToBodyEulers_i(uint16_t id, struct Int32Eulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (int).
Definition state.h:1265
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
Definition state.h:1331
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static void stateSetNedToBodyRMat_f(uint16_t id, struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition state.h:1279
static void stateSetNedToBodyRMat_i(uint16_t id, struct Int32RMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (int).
Definition state.h:1258
static void stateSetFixedWingFlight(bool fixed_wing)
Set if flying as a fixed wing (e.g. a hybrid in forward flight).
Definition state.h:1647
bool fixed_wing_flight
Flying as a fixed wing: the wing gives the lift (the fixedwing firmware, or a hybrid in forward fligh...
Definition state.h:507
static bool stateIsFixedWingFlight(void)
Flying as a fixed wing (always for the fixedwing firmware, in forward flight for hybrids).
Definition state.h:1653
uint16_t attitude_input_filter
Holds the input filter id for attitude.
Definition state.h:411
#define STATE_CHECK_INPUT_AND_RETURN(flag, id)
Helper macro to check input filter and return if not valid input id.
Definition state.h:524
static struct FloatVect3 * stateGetAccelBody_f(void)
Definition state.h:1111
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
struct OrientationReps ned_to_body_orientation
Definition state.h:406
struct State state
Definition state.c:36
void stateInit(void)
Definition state.c:57
Structure holding vehicle state data.
Definition state.h:152
#define POS_LLA_F
Definition state.h:79
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition state.h:206
#define POS_UTM_F
Definition state.h:80
static void stateSetPositionLla_f(uint16_t id, struct LlaCoor_f *lla_pos)
Set position from LLA coordinates (float).
Definition state.h:749
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
Definition state.h:577
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:729
#define POS_LLA_I
Definition state.h:74
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition state.h:271
#define POS_ENU_F
Definition state.h:78
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition state.h:278
#define POS_ECEF_F
Definition state.h:76
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition state.h:220
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition state.h:796
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition state.h:805
void stateCalcPositionNed_f(void)
Definition state.c:588
static void stateSetPositionEnu_i(uint16_t id, struct EnuCoor_i *enu_pos)
Set position from local ENU coordinates (int).
Definition state.h:659
static void stateSetPosition_f(uint16_t id, struct EcefCoor_f *ecef_pos, struct NedCoor_f *ned_pos, struct EnuCoor_f *enu_pos, struct LlaCoor_f *lla_pos, struct UtmCoor_f *utm_pos)
Set multiple position coordinates (float).
Definition state.h:759
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition state.h:248
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition state.h:194
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition state.h:251
struct EcefCoor_f stateGetEcefOrigin_f(void)
Get the ECEF position of the frame origin (float).
Definition state.c:181
uint16_t pos_status
Holds the status bits for all position representations.
Definition state.h:167
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition state.h:185
void stateCalcPositionUtm_f(void)
Definition state.c:520
static void stateSetPositionEnu_f(uint16_t id, struct EnuCoor_f *enu_pos)
Set position from local ENU coordinates (float).
Definition state.h:739
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition state.h:823
void stateCalcPositionEnu_f(void)
Definition state.c:655
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition state.h:239
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:709
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition state.h:264
#define POS_ENU_I
Definition state.h:73
uint16_t origin_input_filter
Holds the input filter id for state origin.
Definition state.h:160
#define POS_ECEF_I
Definition state.h:71
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int).
Definition state.h:587
#define POS_GLOBAL_COORD
Definition state.h:82
void stateCalcPositionEcef_f(void)
Definition state.c:557
#define POS_LOCAL_COORD
Definition state.h:81
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition state.h:213
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition state.h:199
static void stateSetPositionEcef_f(uint16_t id, struct EcefCoor_f *ecef_pos)
Set position from ECEF coordinates (float).
Definition state.h:719
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition state.h:814
#define POS_NED_F
Definition state.h:77
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition state.h:631
float alt_agl_f
Altitude above ground level.
Definition state.h:226
void stateCalcPositionLla_f(void)
Definition state.c:728
static struct EcefCoor_f * stateGetPositionEcef_f(void)
Get position in ECEF coordinates (float).
Definition state.h:841
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
Definition state.c:196
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition state.h:552
void stateCalcPositionEcef_i(void)
Definition state.c:223
static void stateSetPositionEcef_i(uint16_t id, struct EcefCoor_i *ecef_pos)
Set position from ECEF coordinates (int).
Definition state.h:639
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition state.h:261
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
void stateCalcPositionNed_i(void)
Definition state.c:255
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition state.c:422
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition state.h:624
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static void stateSetPositionLla_i(uint16_t id, struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition state.h:669
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
Definition state.h:567
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition state.h:233
static void stateSetPosition_i(uint16_t id, struct EcefCoor_i *ecef_pos, struct NedCoor_i *ned_pos, struct EnuCoor_i *enu_pos, struct LlaCoor_i *lla_pos)
Set multiple position coordinates (int).
Definition state.h:679
uint16_t pos_input_filter
Holds the input filter id for position.
Definition state.h:172
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition state.h:178
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
#define POS_NED_I
Definition state.h:72
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition state.h:649
struct EcefCoor_i stateGetEcefOrigin_i(void)
Get the ECEF position of the frame origin (int).
Definition state.c:166
void stateCalcPositionEnu_i(void)
Definition state.c:335
uint16_t rates_input_filter
Holds the input filter id for rates.
Definition state.h:426
static void stateSetBodyRates_i(uint16_t id, struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition state.h:1361
void stateCalcBodyRates_f(void)
Definition state.c:1377
#define RATE_F
Definition state.h:117
struct FloatRates body_rates_f
Angular rates in body frame.
Definition state.h:438
#define RATE_I
Definition state.h:116
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition state.h:1383
void stateCalcBodyRates_i(void)
Definition state.c:1362
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition state.h:432
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition state.h:421
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static bool stateIsRateValid(void)
Test if rates are valid.
Definition state.h:1353
#define SPEED_ENU_F
Definition state.h:96
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
static void stateSetSpeedEcef_f(uint16_t id, struct EcefCoor_f *ecef_speed)
Set ground speed in ECEF coordinates (float).
Definition state.h:978
#define SPEED_ENU_I
Definition state.h:91
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
void stateCalcSpeedEnu_f(void)
Definition state.c:1083
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition state.h:336
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition state.h:903
static void stateSetSpeedEcef_i(uint16_t id, struct EcefCoor_i *ecef_speed)
Set ground speed in ECEF coordinates (int).
Definition state.h:923
#define SPEED_ECEF_F
Definition state.h:94
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition state.h:1015
void stateCalcSpeedNed_f(void)
Definition state.c:1033
uint16_t speed_input_filter
Holds the input filter id for speed.
Definition state.h:294
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition state.h:1105
static void stateSetSpeed_f(uint16_t id, struct EcefCoor_f *ecef_speed, struct NedCoor_f *ned_speed, struct EnuCoor_f *enu_speed)
Set multiple speed coordinates (float).
Definition state.h:988
#define SPEED_ECEF_I
Definition state.h:89
static void stateSetSpeedEnu_f(uint16_t id, struct EnuCoor_f *enu_speed)
Set ground speed in local ENU coordinates (float).
Definition state.h:968
void stateCalcHorizontalSpeedDir_i(void)
Definition state.c:1002
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1096
#define SPEED_NED_F
Definition state.h:95
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition state.h:306
float h_speed_norm_f
Norm of horizontal ground speed.
Definition state.h:348
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition state.h:325
float h_speed_dir_f
Direction of horizontal ground speed.
Definition state.h:354
void stateCalcHorizontalSpeedNorm_f(void)
Definition state.c:1158
void stateCalcHorizontalSpeedNorm_i(void)
Definition state.c:956
#define SPEED_LOCAL_COORD
Definition state.h:99
static int32_t stateGetHorizontalSpeedDir_i(void)
Get dir of horizontal ground speed (int).
Definition state.h:1051
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition state.h:331
static struct EcefCoor_f * stateGetSpeedEcef_f(void)
Get ground speed in ECEF coordinates (float).
Definition state.h:1078
void stateCalcSpeedEcef_f(void)
Definition state.c:1133
#define SPEED_HDIR_I
Definition state.h:93
void stateCalcSpeedEnu_i(void)
Definition state.c:879
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
#define SPEED_HNORM_F
Definition state.h:97
#define SPEED_NED_I
Definition state.h:90
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition state.h:300
void stateCalcHorizontalSpeedDir_f(void)
Definition state.c:1185
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition state.h:312
static struct EcefCoor_i * stateGetSpeedEcef_i(void)
Get ground speed in ECEF coordinates (int).
Definition state.h:1033
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition state.h:289
static void stateSetSpeedEnu_i(uint16_t id, struct EnuCoor_i *enu_speed)
Set ground speed in local ENU coordinates (int).
Definition state.h:913
static void stateSetSpeed_i(uint16_t id, struct EcefCoor_i *ecef_speed, struct NedCoor_i *ned_speed, struct EnuCoor_i *enu_speed)
Set multiple speed coordinates (int).
Definition state.h:933
void stateCalcSpeedNed_i(void)
Definition state.c:829
#define SPEED_HNORM_I
Definition state.h:92
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition state.h:1024
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition state.h:318
static uint32_t stateGetHorizontalSpeedNorm_i(void)
Get norm of horizontal ground speed (int).
Definition state.h:1042
#define SPEED_HDIR_F
Definition state.h:98
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition state.h:342
void stateCalcSpeedEcef_i(void)
Definition state.c:931
static bool stateIsSideslipValid(void)
test if sideslip is available.
Definition state.h:1448
static void stateSetAngleOfAttack_f(uint16_t id, float aoa)
Set angle of attack in radians (float).
Definition state.h:1522
#define WINDSPEED_I
Definition state.h:124
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition state.h:1424
static void stateSetSideslip_f(uint16_t id, float sideslip)
Set sideslip angle in radians (float).
Definition state.h:1533
int32_t airspeed_i
Norm of relative wind speed.
Definition state.h:469
#define DOWNWIND_F
Definition state.h:128
union State::@071130125207041214011307061206174333016062274057 windspeed_f
Horizontal windspeed.
union State::@230375120273352331277040361213172373156331073237 windspeed_i
Horizontal windspeed in north/east/down.
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition state.h:1633
#define WINDSPEED_F
Definition state.h:127
float airspeed_f
Norm of relative air speed.
Definition state.h:484
static void stateSetVerticalWindspeed_f(uint16_t id, float v_windspeed)
Set vertical windspeed (float).
Definition state.h:1500
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition state.h:1436
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition state.h:449
#define AIRSPEED_I
Definition state.h:126
static void stateSetVerticalWindspeed_i(uint16_t id, int32_t v_windspeed)
Set vertical windspeed (int).
Definition state.h:1467
static struct FloatVect3 * stateGetWindspeed_f(void)
Get windspeed (float).
Definition state.h:1603
#define SIDESLIP_F
Definition state.h:131
static void stateSetAirspeed_i(uint16_t id, int32_t airspeed)
Set airspeed (int).
Definition state.h:1478
void stateCalcAirspeed_i(void)
Definition state.c:1434
static void stateSetHorizontalWindspeed_i(uint16_t id, struct Int32Vect2 *h_windspeed)
Set horizontal windspeed (int).
Definition state.h:1456
void stateCalcVerticalWindspeed_i(void)
Definition state.c:1419
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition state.h:1585
static struct Int32Vect2 * stateGetHorizontalWindspeed_i(void)
Get horizontal windspeed (int).
Definition state.h:1546
static float stateGetVerticalWindspeed_i(void)
Get vertical windspeed (int).
Definition state.h:1555
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition state.h:1624
void stateCalcAirspeed_f(void)
Definition state.c:1480
#define AIRSPEED_F
Definition state.h:129
float sideslip_f
Sideslip angle Unit: rad.
Definition state.h:496
uint16_t wind_air_input_filter
Holds the input filter id for air data.
Definition state.h:454
void stateCalcHorizontalWindspeed_f(void)
Definition state.c:1449
static float stateGetVerticalWindspeed_f(void)
Get vertical windspeed (float).
Definition state.h:1594
static int32_t stateGetAirspeed_i(void)
Get airspeed (int).
Definition state.h:1576
static struct Int32Vect3 * stateGetWindspeed_i(void)
Get windspeed (int).
Definition state.h:1564
static void stateSetHorizontalWindspeed_f(uint16_t id, struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition state.h:1489
void stateCalcHorizontalWindspeed_i(void)
Definition state.c:1403
float angle_of_attack_f
Angle of attack Unit: rad.
Definition state.h:490
#define AOA_F
Definition state.h:130
static bool stateIsAngleOfAttackValid(void)
test if angle of attack is available.
Definition state.h:1442
#define DOWNWIND_I
Definition state.h:125
static void stateSetAirspeed_f(uint16_t id, float airspeed)
Set airspeed (float).
Definition state.h:1511
void stateCalcVerticalWindspeed_f(void)
Definition state.c:1465
static bool stateIsVerticalWindspeedValid(void)
test if vertical wind speed is available.
Definition state.h:1430
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Paparazzi floating point math for geodetic calculations.
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi fixed point math for geodetic calculations.
Generic orientation representation and conversions.
struct FloatVect3 body_accel_f
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.