Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
31 #ifndef STATE_H
32 #define STATE_H
33 
34 #include "math/pprz_algebra_int.h"
36 #include "math/pprz_geodetic_int.h"
39 
40 #include "std.h"
41 #include <string.h>
42 
67 #define POS_ECEF_I 0
68 #define POS_NED_I 1
69 #define POS_ENU_I 2
70 #define POS_LLA_I 3
71 #define POS_UTM_I 4
72 #define POS_ECEF_F 5
73 #define POS_NED_F 6
74 #define POS_ENU_F 7
75 #define POS_LLA_F 8
76 #define POS_UTM_F 9
77 #define POS_LOCAL_COORD ((1<<POS_NED_I)|(1<<POS_NED_F)|(1<<POS_ENU_I)|(1<<POS_ENU_F))
78 #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))
79 
85 #define SPEED_ECEF_I 0
86 #define SPEED_NED_I 1
87 #define SPEED_ENU_I 2
88 #define SPEED_HNORM_I 3
89 #define SPEED_HDIR_I 4
90 #define SPEED_ECEF_F 5
91 #define SPEED_NED_F 6
92 #define SPEED_ENU_F 7
93 #define SPEED_HNORM_F 8
94 #define SPEED_HDIR_F 9
95 #define SPEED_LOCAL_COORD ((1<<SPEED_NED_I)|(1<<SPEED_ENU_I)|(1<<SPEED_NED_F)|(1<<SPEED_ENU_F))
96 
102 #define ACCEL_ECEF_I 0
103 #define ACCEL_NED_I 1
104 #define ACCEL_ECEF_F 2
105 #define ACCEL_NED_F 3
106 
112 #define RATE_I 0
113 #define RATE_F 1
114 
120 #define WINDSPEED_I 0
121 #define DOWNWIND_I 1
122 #define AIRSPEED_I 2
123 #define WINDSPEED_F 3
124 #define DOWNWIND_F 4
125 #define AIRSPEED_F 5
126 #define AOA_F 6
127 #define SIDESLIP_F 7
128 
134 struct State {
135 
145 
151 
158 
167 
172 
179 
186 
193 
198  float alt_agl_f;
199 
206 
212 
221 
224 
234 
237 
244 
262 
268 
274 
280 
286 
293 
299 
304 
310 
316 
333 
339 
345 
351 
357 
369 
370 
379 
385 
402 
407  union {
408  struct Int32Vect3 vect3;
409  struct Int32Vect2 vect2;
410  } windspeed_i;
411 
417 
422  union {
423  struct FloatVect3 vect3;
424  struct FloatVect2 vect2;
425  } windspeed_f;
426 
431  float airspeed_f;
432 
438 
443  float sideslip_f;
444 
447 };
448 
449 extern struct State state;
450 
451 extern void stateInit(void);
452 
456 static inline void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
458 {
460  /* convert to float */
465 
466  /* clear bits for all local frame representations */
469  ClearBit(state.accel_status, ACCEL_NED_I);
470  ClearBit(state.accel_status, ACCEL_NED_F);
471 
472  state.ned_initialized_i = true;
473  state.ned_initialized_f = true;
474 }
475 
477 static inline void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
478 {
479  state.utm_origin_f = *utm_def;
480  state.utm_initialized_f = true;
481 
482  /* clear bits for all local frame representations */
485  ClearBit(state.accel_status, ACCEL_NED_I);
486  ClearBit(state.accel_status, ACCEL_NED_F);
487 }
488 /*******************************************************************************
489  * *
490  * Set and Get functions for the POSITION representations *
491  * *
492  ******************************************************************************/
493 
494 /************* declaration of transformation functions ************/
495 extern void stateCalcPositionEcef_i(void);
496 extern void stateCalcPositionNed_i(void);
497 extern void stateCalcPositionEnu_i(void);
498 extern void stateCalcPositionLla_i(void);
499 extern void stateCalcPositionUtm_f(void);
500 extern void stateCalcPositionEcef_f(void);
501 extern void stateCalcPositionNed_f(void);
502 extern void stateCalcPositionEnu_f(void);
503 extern void stateCalcPositionLla_f(void);
504 
505 /*********************** validity test functions ******************/
506 
508 static inline bool stateIsLocalCoordinateValid(void)
509 {
511  && (state.pos_status & (POS_LOCAL_COORD)));
512 }
513 
515 static inline bool stateIsGlobalCoordinateValid(void)
516 {
518 }
519 
520 /************************ Set functions ****************************/
521 
523 static inline void stateSetPositionEcef_i(struct EcefCoor_i *ecef_pos)
524 {
525  VECT3_COPY(state.ecef_pos_i, *ecef_pos);
526  /* clear bits for all position representations and only set the new one */
527  state.pos_status = (1 << POS_ECEF_I);
528 }
529 
531 static inline void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
532 {
533  VECT3_COPY(state.ned_pos_i, *ned_pos);
534  /* clear bits for all position representations and only set the new one */
535  state.pos_status = (1 << POS_NED_I);
536 }
537 
539 static inline void stateSetPositionEnu_i(struct EnuCoor_i *enu_pos)
540 {
541  VECT3_COPY(state.enu_pos_i, *enu_pos);
542  /* clear bits for all position representations and only set the new one */
543  state.pos_status = (1 << POS_ENU_I);
544 }
545 
547 static inline void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
548 {
549  LLA_COPY(state.lla_pos_i, *lla_pos);
550  /* clear bits for all position representations and only set the new one */
551  state.pos_status = (1 << POS_LLA_I);
552 }
553 
555 static inline void stateSetPosition_i(
556  struct EcefCoor_i *ecef_pos,
557  struct NedCoor_i *ned_pos,
558  struct EnuCoor_i *enu_pos,
559  struct LlaCoor_i *lla_pos)
560 {
561  /* clear all status bit */
562  state.pos_status = 0;
563  if (ecef_pos != NULL) {
564  VECT3_COPY(state.ecef_pos_i, *ecef_pos);
565  state.pos_status |= (1 << POS_ECEF_I);
566  }
567  if (ned_pos != NULL) {
568  VECT3_COPY(state.ned_pos_i, *ned_pos);
569  state.pos_status |= (1 << POS_NED_I);
570  }
571  if (enu_pos != NULL) {
572  VECT3_COPY(state.enu_pos_i, *enu_pos);
573  state.pos_status |= (1 << POS_ENU_I);
574  }
575  if (lla_pos != NULL) {
576  LLA_COPY(state.lla_pos_i, *lla_pos);
577  state.pos_status |= (1 << POS_LLA_I);
578  }
579 }
580 
582 static inline void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
583 {
584  state.utm_pos_f = *utm_pos;
585  /* clear bits for all position representations and only set the new one */
586  state.pos_status = (1 << POS_UTM_F);
587 }
588 
590 static inline void stateSetPositionEcef_f(struct EcefCoor_f *ecef_pos)
591 {
592  VECT3_COPY(state.ecef_pos_f, *ecef_pos);
593  /* clear bits for all position representations and only set the new one */
594  state.pos_status = (1 << POS_ECEF_F);
595 }
596 
598 static inline void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
599 {
600  VECT3_COPY(state.ned_pos_f, *ned_pos);
601  /* clear bits for all position representations and only set the new one */
602  state.pos_status = (1 << POS_NED_F);
603 }
604 
606 static inline void stateSetPositionEnu_f(struct EnuCoor_f *enu_pos)
607 {
608  VECT3_COPY(state.enu_pos_f, *enu_pos);
609  /* clear bits for all position representations and only set the new one */
610  state.pos_status = (1 << POS_ENU_F);
611 }
612 
614 static inline void stateSetPositionLla_f(struct LlaCoor_f *lla_pos)
615 {
616  LLA_COPY(state.lla_pos_f, *lla_pos);
617  /* clear bits for all position representations and only set the new one */
618  state.pos_status = (1 << POS_LLA_F);
619 }
620 
622 static inline void stateSetPosition_f(
623  struct EcefCoor_f *ecef_pos,
624  struct NedCoor_f *ned_pos,
625  struct EnuCoor_f *enu_pos,
626  struct LlaCoor_f *lla_pos,
627  struct UtmCoor_f *utm_pos)
628 {
629  /* clear all status bit */
630  state.pos_status = 0;
631  if (ecef_pos != NULL) {
632  VECT3_COPY(state.ecef_pos_f, *ecef_pos);
633  state.pos_status |= (1 << POS_ECEF_F);
634  }
635  if (ned_pos != NULL) {
636  VECT3_COPY(state.ned_pos_f, *ned_pos);
637  state.pos_status |= (1 << POS_NED_F);
638  }
639  if (enu_pos != NULL) {
640  VECT3_COPY(state.enu_pos_f, *enu_pos);
641  state.pos_status |= (1 << POS_ENU_F);
642  }
643  if (lla_pos != NULL) {
644  LLA_COPY(state.lla_pos_f, *lla_pos);
645  state.pos_status |= (1 << POS_LLA_F);
646  }
647  if (utm_pos != NULL) {
648  state.utm_pos_f = *utm_pos;
649  state.pos_status |= (1 << POS_UTM_F);
650  }
651 }
652 
653 /************************ Get functions ****************************/
654 
656 static inline struct EcefCoor_i *stateGetPositionEcef_i(void)
657 {
658  if (!bit_is_set(state.pos_status, POS_ECEF_I)) {
660  }
661  return &state.ecef_pos_i;
662 }
663 
665 static inline struct NedCoor_i *stateGetPositionNed_i(void)
666 {
667  if (!bit_is_set(state.pos_status, POS_NED_I)) {
669  }
670  return &state.ned_pos_i;
671 }
672 
674 static inline struct EnuCoor_i *stateGetPositionEnu_i(void)
675 {
676  if (!bit_is_set(state.pos_status, POS_ENU_I)) {
678  }
679  return &state.enu_pos_i;
680 }
681 
683 static inline struct LlaCoor_i *stateGetPositionLla_i(void)
684 {
685  if (!bit_is_set(state.pos_status, POS_LLA_I)) {
687  }
688  return &state.lla_pos_i;
689 }
690 
692 static inline struct UtmCoor_f *stateGetPositionUtm_f(void)
693 {
694  if (!bit_is_set(state.pos_status, POS_UTM_F)) {
696  }
697  return &state.utm_pos_f;
698 }
699 
701 static inline struct EcefCoor_f *stateGetPositionEcef_f(void)
702 {
703  if (!bit_is_set(state.pos_status, POS_ECEF_F)) {
705  }
706  return &state.ecef_pos_f;
707 }
708 
710 static inline struct NedCoor_f *stateGetPositionNed_f(void)
711 {
712  if (!bit_is_set(state.pos_status, POS_NED_F)) {
714  }
715  return &state.ned_pos_f;
716 }
717 
719 static inline struct EnuCoor_f *stateGetPositionEnu_f(void)
720 {
721  if (!bit_is_set(state.pos_status, POS_ENU_F)) {
723  }
724  return &state.enu_pos_f;
725 }
726 
728 static inline struct LlaCoor_f *stateGetPositionLla_f(void)
729 {
730  if (!bit_is_set(state.pos_status, POS_LLA_F)) {
732  }
733  return &state.lla_pos_f;
734 }
735 
740 /******************************************************************************
741  * *
742  * Set and Get functions for the SPEED representations *
743  * *
744  *****************************************************************************/
748 /************* declaration of transformation functions ************/
749 extern void stateCalcSpeedNed_i(void);
750 extern void stateCalcSpeedEnu_i(void);
751 extern void stateCalcSpeedEcef_i(void);
752 extern void stateCalcHorizontalSpeedNorm_i(void);
753 extern void stateCalcHorizontalSpeedDir_i(void);
754 extern void stateCalcSpeedNed_f(void);
755 extern void stateCalcSpeedEnu_f(void);
756 extern void stateCalcSpeedEcef_f(void);
757 extern void stateCalcHorizontalSpeedNorm_f(void);
758 extern void stateCalcHorizontalSpeedDir_f(void);
759 
760 /************************ Set functions ****************************/
761 
763 static inline void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
764 {
765  VECT3_COPY(state.ned_speed_i, *ned_speed);
766  /* clear bits for all speed representations and only set the new one */
767  state.speed_status = (1 << SPEED_NED_I);
768 }
769 
771 static inline void stateSetSpeedEnu_i(struct EnuCoor_i *enu_speed)
772 {
773  VECT3_COPY(state.enu_speed_i, *enu_speed);
774  /* clear bits for all speed representations and only set the new one */
775  state.speed_status = (1 << SPEED_ENU_I);
776 }
777 
779 static inline void stateSetSpeedEcef_i(struct EcefCoor_i *ecef_speed)
780 {
781  VECT3_COPY(state.ecef_speed_i, *ecef_speed);
782  /* clear bits for all speed representations and only set the new one */
784 }
785 
787 static inline void stateSetSpeed_i(
788  struct EcefCoor_i *ecef_speed,
789  struct NedCoor_i *ned_speed,
790  struct EnuCoor_i *enu_speed)
791 {
792  /* clear all status bit */
793  state.speed_status = 0;
794  if (ecef_speed != NULL) {
795  VECT3_COPY(state.ecef_speed_i, *ecef_speed);
796  state.speed_status |= (1 << SPEED_ECEF_I);
797  }
798  if (ned_speed != NULL) {
799  VECT3_COPY(state.ned_speed_i, *ned_speed);
800  state.speed_status |= (1 << SPEED_NED_I);
801  }
802  if (enu_speed != NULL) {
803  VECT3_COPY(state.enu_speed_i, *enu_speed);
804  state.speed_status |= (1 << SPEED_ENU_I);
805  }
806 }
807 
809 static inline void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
810 {
811  VECT3_COPY(state.ned_speed_f, *ned_speed);
812  /* clear bits for all speed representations and only set the new one */
813  state.speed_status = (1 << SPEED_NED_F);
814 }
815 
817 static inline void stateSetSpeedEnu_f(struct EnuCoor_f *enu_speed)
818 {
819  VECT3_COPY(state.enu_speed_f, *enu_speed);
820  /* clear bits for all speed representations and only set the new one */
821  state.speed_status = (1 << SPEED_ENU_F);
822 }
823 
825 static inline void stateSetSpeedEcef_f(struct EcefCoor_f *ecef_speed)
826 {
827  VECT3_COPY(state.ecef_speed_f, *ecef_speed);
828  /* clear bits for all speed representations and only set the new one */
830 }
831 
833 static inline void stateSetSpeed_f(
834  struct EcefCoor_f *ecef_speed,
835  struct NedCoor_f *ned_speed,
836  struct EnuCoor_f *enu_speed)
837 {
838  /* clear all status bit */
839  state.speed_status = 0;
840  if (ecef_speed != NULL) {
841  VECT3_COPY(state.ecef_speed_f, *ecef_speed);
842  state.speed_status |= (1 << SPEED_ECEF_F);
843  }
844  if (ned_speed != NULL) {
845  VECT3_COPY(state.ned_speed_f, *ned_speed);
846  state.speed_status |= (1 << SPEED_NED_F);
847  }
848  if (enu_speed != NULL) {
849  VECT3_COPY(state.enu_speed_f, *enu_speed);
850  state.speed_status |= (1 << SPEED_ENU_F);
851  }
852 }
853 
855 static inline void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
856 {
857  VECT3_COPY(state.body_accel_i, *body_accel);
858 }
859 
860 /************************ Get functions ****************************/
861 
863 static inline struct NedCoor_i *stateGetSpeedNed_i(void)
864 {
865  if (!bit_is_set(state.speed_status, SPEED_NED_I)) {
867  }
868  return &state.ned_speed_i;
869 }
870 
872 static inline struct EnuCoor_i *stateGetSpeedEnu_i(void)
873 {
874  if (!bit_is_set(state.speed_status, SPEED_ENU_I)) {
876  }
877  return &state.enu_speed_i;
878 }
879 
881 static inline struct EcefCoor_i *stateGetSpeedEcef_i(void)
882 {
883  if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) {
885  }
886  return &state.ecef_speed_i;
887 }
888 
891 {
892  if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) {
894  }
895  return state.h_speed_norm_i;
896 }
897 
900 {
901  if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) {
903  }
904  return state.h_speed_dir_i;
905 }
906 
908 static inline struct NedCoor_f *stateGetSpeedNed_f(void)
909 {
910  if (!bit_is_set(state.speed_status, SPEED_NED_F)) {
912  }
913  return &state.ned_speed_f;
914 }
915 
917 static inline struct EnuCoor_f *stateGetSpeedEnu_f(void)
918 {
919  if (!bit_is_set(state.speed_status, SPEED_ENU_F)) {
921  }
922  return &state.enu_speed_f;
923 }
924 
926 static inline struct EcefCoor_f *stateGetSpeedEcef_f(void)
927 {
928  if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) {
930  }
931  return &state.ecef_speed_f;
932 }
933 
935 static inline float stateGetHorizontalSpeedNorm_f(void)
936 {
937  if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) {
939  }
940  return state.h_speed_norm_f;
941 }
942 
944 static inline float stateGetHorizontalSpeedDir_f(void)
945 {
946  if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) {
948  }
949  return state.h_speed_dir_f;
950 }
951 
953 static inline struct Int32Vect3 *stateGetAccelBody_i(void)
954 {
955  return &state.body_accel_i;
956 }
961 /******************************************************************************
962  * *
963  * Set and Get functions for the ACCELERATION representations *
964  * *
965  *****************************************************************************/
969 /************* declaration of transformation functions ************/
970 extern void stateCalcAccelNed_i(void);
971 extern void stateCalcAccelEcef_i(void);
972 extern void stateCalcAccelNed_f(void);
973 extern void stateCalcAccelEcef_f(void);
974 
975 /*********************** validity test functions ******************/
976 
978 static inline bool stateIsAccelValid(void)
979 {
980  return (state.accel_status);
981 }
982 
983 /************************ Set functions ****************************/
984 
986 static inline void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
987 {
988  VECT3_COPY(state.ned_accel_i, *ned_accel);
989  /* clear bits for all accel representations and only set the new one */
990  state.accel_status = (1 << ACCEL_NED_I);
991 }
992 
994 static inline void stateSetAccelEcef_i(struct EcefCoor_i *ecef_accel)
995 {
996  VECT3_COPY(state.ecef_accel_i, *ecef_accel);
997  /* clear bits for all accel representations and only set the new one */
999 }
1000 
1002 static inline void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
1003 {
1004  VECT3_COPY(state.ned_accel_f, *ned_accel);
1005  /* clear bits for all accel representations and only set the new one */
1006  state.accel_status = (1 << ACCEL_NED_F);
1007 }
1008 
1010 static inline void stateSetAccelEcef_f(struct EcefCoor_f *ecef_accel)
1011 {
1012  VECT3_COPY(state.ecef_accel_f, *ecef_accel);
1013  /* clear bits for all accel representations and only set the new one */
1014  state.accel_status = (1 << ACCEL_ECEF_F);
1015 }
1016 
1017 /************************ Get functions ****************************/
1018 
1020 static inline struct NedCoor_i *stateGetAccelNed_i(void)
1021 {
1022  if (!bit_is_set(state.accel_status, ACCEL_NED_I)) {
1024  }
1025  return &state.ned_accel_i;
1026 }
1027 
1029 static inline struct EcefCoor_i *stateGetAccelEcef_i(void)
1030 {
1031  if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
1033  }
1034  return &state.ecef_accel_i;
1035 }
1036 
1038 static inline struct NedCoor_f *stateGetAccelNed_f(void)
1039 {
1040  if (!bit_is_set(state.accel_status, ACCEL_NED_F)) {
1042  }
1043  return &state.ned_accel_f;
1044 }
1045 
1047 static inline struct EcefCoor_f *stateGetAccelEcef_f(void)
1048 {
1049  if (!bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
1051  }
1052  return &state.ecef_accel_f;
1053 }
1056 /******************************************************************************
1057 * *
1058 * Set and Get functions for the ATTITUDE representations *
1059 * (Calls the functions in math/pprz_orientation_conversion) *
1060 * *
1061 *****************************************************************************/
1064 /*********************** validity test functions ******************/
1065 
1067 static inline bool stateIsAttitudeValid(void)
1068 {
1070 }
1071 
1072 /************************ Set functions ****************************/
1073 
1075 static inline void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
1076 {
1078 }
1079 
1081 static inline void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat)
1082 {
1084 }
1085 
1087 static inline void stateSetNedToBodyEulers_i(struct Int32Eulers *ned_to_body_eulers)
1088 {
1090 }
1091 
1093 static inline void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
1094 {
1096 }
1097 
1099 static inline void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
1100 {
1102 }
1103 
1105 static inline void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
1106 {
1108 }
1109 
1110 /************************ Get functions ****************************/
1111 
1113 static inline struct Int32Quat *stateGetNedToBodyQuat_i(void)
1114 {
1116 }
1117 
1119 static inline struct Int32RMat *stateGetNedToBodyRMat_i(void)
1120 {
1122 }
1123 
1125 static inline struct Int32Eulers *stateGetNedToBodyEulers_i(void)
1126 {
1128 }
1129 
1131 static inline struct FloatQuat *stateGetNedToBodyQuat_f(void)
1132 {
1134 }
1135 
1137 static inline struct FloatRMat *stateGetNedToBodyRMat_f(void)
1138 {
1140 }
1141 
1143 static inline struct FloatEulers *stateGetNedToBodyEulers_f(void)
1144 {
1146 }
1150 /******************************************************************************
1151  * *
1152  * Set and Get functions for the ANGULAR RATE representations *
1153  * *
1154  *****************************************************************************/
1158 /************* declaration of transformation functions ************/
1159 extern void stateCalcBodyRates_i(void);
1160 extern void stateCalcBodyRates_f(void);
1161 
1162 /*********************** validity test functions ******************/
1163 
1165 static inline bool stateIsRateValid(void)
1166 {
1167  return (state.rate_status);
1168 }
1169 
1170 /************************ Set functions ****************************/
1171 
1173 static inline void stateSetBodyRates_i(struct Int32Rates *body_rate)
1174 {
1175  RATES_COPY(state.body_rates_i, *body_rate);
1176  /* clear bits for all attitude representations and only set the new one */
1177  state.rate_status = (1 << RATE_I);
1178 }
1179 
1181 static inline void stateSetBodyRates_f(struct FloatRates *body_rate)
1182 {
1183  RATES_COPY(state.body_rates_f, *body_rate);
1184  /* clear bits for all attitude representations and only set the new one */
1185  state.rate_status = (1 << RATE_F);
1186 }
1187 
1188 /************************ Get functions ****************************/
1189 
1191 static inline struct Int32Rates *stateGetBodyRates_i(void)
1192 {
1193  if (!bit_is_set(state.rate_status, RATE_I)) {
1195  }
1196  return &state.body_rates_i;
1197 }
1198 
1200 static inline struct FloatRates *stateGetBodyRates_f(void)
1201 {
1202  if (!bit_is_set(state.rate_status, RATE_F)) {
1204  }
1205  return &state.body_rates_f;
1206 }
1207 
1212 /******************************************************************************
1213  * *
1214  * Set and Get functions for the WIND- AND AIRSPEED representations *
1215  * *
1216  *****************************************************************************/
1220 /************* declaration of transformation functions ************/
1221 extern void stateCalcHorizontalWindspeed_i(void);
1222 extern void stateCalcVerticalWindspeed_i(void);
1223 extern void stateCalcAirspeed_i(void);
1224 extern void stateCalcHorizontalWindspeed_f(void);
1225 extern void stateCalcVerticalWindspeed_f(void);
1226 extern void stateCalcAirspeed_f(void);
1227 
1228 
1229 /************************ validity test function *******************/
1230 
1232 static inline bool stateIsWindspeedValid(void)
1233 {
1234  return (state.wind_air_status &= ~((1 << WINDSPEED_I) | (1 << WINDSPEED_F)));
1235 }
1236 
1238 static inline bool stateIsVerticalWindspeedValid(void)
1239 {
1240  return (state.wind_air_status &= ~((1 << DOWNWIND_I) | (1 << DOWNWIND_F)));
1241 }
1242 
1244 static inline bool stateIsAirspeedValid(void)
1245 {
1246  return (state.wind_air_status &= ~((1 << AIRSPEED_I) | (1 << AIRSPEED_F)));
1247 }
1248 
1250 static inline bool stateIsAngleOfAttackValid(void)
1251 {
1252  return (state.wind_air_status &= ~(1 << AOA_F));
1253 }
1254 
1256 static inline bool stateIsSideslipValid(void)
1257 {
1258  return (state.wind_air_status &= ~(1 << SIDESLIP_F));
1259 }
1260 
1261 /************************ Set functions ****************************/
1262 
1264 static inline void stateSetHorizontalWindspeed_i(struct Int32Vect2 *h_windspeed)
1265 {
1266  VECT2_COPY(state.windspeed_i.vect2, *h_windspeed);
1267  /* clear bits for all horizontal windspeed representations and only set the new one */
1268  ClearBit(state.wind_air_status, WINDSPEED_F);
1270 }
1271 
1273 static inline void stateSetVerticalWindspeed_i(int32_t v_windspeed)
1274 {
1275  state.windspeed_i.vect3.z = v_windspeed;
1276  /* clear bits for all vertical windspeed representations and only set the new one */
1277  ClearBit(state.wind_air_status, DOWNWIND_F);
1278  SetBit(state.wind_air_status, DOWNWIND_I);
1279 }
1280 
1282 static inline void stateSetAirspeed_i(int32_t airspeed)
1283 {
1284  state.airspeed_i = airspeed;
1285  /* clear bits for all airspeed representations and only set the new one */
1286  ClearBit(state.wind_air_status, AIRSPEED_F);
1287  SetBit(state.wind_air_status, AIRSPEED_I);
1288 }
1289 
1291 static inline void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
1292 {
1293  VECT2_COPY(state.windspeed_f.vect2, *h_windspeed);
1294  /* clear bits for all horizontal windspeed representations and only set the new one */
1295  ClearBit(state.wind_air_status, WINDSPEED_I);
1297 }
1298 
1300 static inline void stateSetVerticalWindspeed_f(float v_windspeed)
1301 {
1302  state.windspeed_f.vect3.z = v_windspeed;
1303  /* clear bits for all vertical windspeed representations and only set the new one */
1304  ClearBit(state.wind_air_status, DOWNWIND_I);
1305  SetBit(state.wind_air_status, DOWNWIND_F);
1306 }
1307 
1309 static inline void stateSetAirspeed_f(float airspeed)
1310 {
1311  state.airspeed_f = airspeed;
1312  /* clear bits for all airspeed representations and only set the new one */
1313  ClearBit(state.wind_air_status, AIRSPEED_I);
1314  SetBit(state.wind_air_status, AIRSPEED_F);
1315 }
1316 
1318 static inline void stateSetAngleOfAttack_f(float aoa)
1319 {
1320  state.angle_of_attack_f = aoa;
1321  /* clear bits for all AOA representations and only set the new one */
1323  SetBit(state.wind_air_status, AOA_F);
1324 }
1325 
1327 static inline void stateSetSideslip_f(float sideslip)
1328 {
1329  state.sideslip_f = sideslip;
1330  /* clear bits for all sideslip representations and only set the new one */
1332  SetBit(state.wind_air_status, SIDESLIP_F);
1333 }
1334 
1335 /************************ Get functions ****************************/
1336 
1338 static inline struct Int32Vect2 *stateGetHorizontalWindspeed_i(void)
1339 {
1340  if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1342  }
1343  return &state.windspeed_i.vect2;
1344 }
1345 
1347 static inline float stateGetVerticalWindspeed_i(void)
1348 {
1349  if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1351  }
1352  return state.windspeed_i.vect3.z;
1353 }
1354 
1356 static inline struct Int32Vect3 *stateGetWindspeed_i(void)
1357 {
1358  if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
1360  }
1361  if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
1363  }
1364  return &state.windspeed_i.vect3;
1365 }
1366 
1368 static inline int32_t stateGetAirspeed_i(void)
1369 {
1370  if (!bit_is_set(state.wind_air_status, AIRSPEED_I)) {
1372  }
1373  return state.airspeed_i;
1374 }
1375 
1377 static inline struct FloatVect2 *stateGetHorizontalWindspeed_f(void)
1378 {
1379  if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1381  }
1382  return &state.windspeed_f.vect2;
1383 }
1384 
1386 static inline float stateGetVerticalWindspeed_f(void)
1387 {
1388  if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1390  }
1391  return state.windspeed_f.vect3.z;
1392 }
1393 
1395 static inline struct FloatVect3 *stateGetWindspeed_f(void)
1396 {
1397  if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
1399  }
1400  if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
1402  }
1403  return &state.windspeed_f.vect3;
1404 }
1405 
1407 static inline float stateGetAirspeed_f(void)
1408 {
1409  if (!bit_is_set(state.wind_air_status, AIRSPEED_F)) {
1411  }
1412  return state.airspeed_f;
1413 }
1414 
1416 static inline float stateGetAngleOfAttack_f(void)
1417 {
1419 // if (!bit_is_set(state.wind_air_status, AOA_F))
1420 // stateCalcAOA_f();
1421  return state.angle_of_attack_f;
1422 }
1423 
1425 static inline float stateGetSideslip_f(void)
1426 {
1428 // if (!bit_is_set(state.wind_air_status, SIDESLIP_F))
1429 // stateCalcSideslip_f();
1430  return state.sideslip_f;
1431 }
1432 
1438 #endif /* STATE_H */
stateSetAccelNed_i
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
stateCalcAccelEcef_i
void stateCalcAccelEcef_i(void)
Definition: state.c:1101
State::ned_to_body_orientation
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
stateSetAirspeed_f
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
stateCalcSpeedNed_f
void stateCalcSpeedNed_f(void)
Definition: state.c:878
stateCalcAccelEcef_f
void stateCalcAccelEcef_f(void)
Definition: state.c:1166
State::airspeed_f
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
OrientationReps
Definition: pprz_orientation_conversion.h:79
uint16_t
unsigned short uint16_t
Definition: types.h:16
LtpDef_f
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_float.h:93
State::pos_status
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:144
orientationSetEulers_i
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
Definition: pprz_orientation_conversion.h:165
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
stateCalcPositionNed_i
void stateCalcPositionNed_i(void)
Definition: state.c:100
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
stateCalcSpeedEnu_i
void stateCalcSpeedEnu_i(void)
Definition: state.c:724
stateGetPositionNed_f
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
stateSetPositionEcef_i
static void stateSetPositionEcef_i(struct EcefCoor_i *ecef_pos)
Set position from ECEF coordinates (int).
Definition: state.h:523
stateInit
void stateInit(void)
Definition: state.c:43
State::rate_status
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
SPEED_HNORM_F
#define SPEED_HNORM_F
Definition: state.h:93
State::ecef_accel_f
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
State::speed_status
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
stateCalcSpeedEnu_f
void stateCalcSpeedEnu_f(void)
Definition: state.c:928
orientationSetRMat_f
static void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: pprz_orientation_conversion.h:181
stateIsVerticalWindspeedValid
static bool stateIsVerticalWindspeedValid(void)
test if vertical wind speed is available.
Definition: state.h:1238
stateIsLocalCoordinateValid
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
State::ned_accel_f
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
DOWNWIND_F
#define DOWNWIND_F
Definition: state.h:124
State::angle_of_attack_f
float angle_of_attack_f
Angle of attack Unit: rad.
Definition: state.h:437
stateCalcPositionLla_i
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:267
ACCEL_NED_I
#define ACCEL_NED_I
Definition: state.h:103
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
LLA_COPY
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:58
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
State::h_speed_dir_f
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
stateGetAccelNed_f
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
stateCalcPositionLla_f
void stateCalcPositionLla_f(void)
Definition: state.c:573
State::lla_pos_f
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
stateIsAccelValid
static bool stateIsAccelValid(void)
Test if accelerations are valid.
Definition: state.h:978
State::accel_status
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
pprz_geodetic_int.h
Paparazzi fixed point math for geodetic calculations.
stateSetSpeedEcef_i
static void stateSetSpeedEcef_i(struct EcefCoor_i *ecef_speed)
Set ground speed in ECEF coordinates (int).
Definition: state.h:779
State::ecef_pos_i
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:150
stateCalcHorizontalWindspeed_f
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1294
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
State::h_speed_dir_i
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
stateCalcSpeedEcef_i
void stateCalcSpeedEcef_i(void)
Definition: state.c:776
stateGetPositionEnu_i
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
ACCEL_ECEF_I
#define ACCEL_ECEF_I
Definition: state.h:102
stateCalcBodyRates_i
void stateCalcBodyRates_i(void)
Definition: state.c:1207
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetPositionLla_i
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
uint32_t
unsigned long uint32_t
Definition: types.h:18
POS_ECEF_F
#define POS_ECEF_F
Definition: state.h:72
stateCalcBodyRates_f
void stateCalcBodyRates_f(void)
Definition: state.c:1222
stateCalcHorizontalSpeedNorm_i
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:801
stateCalcAccelNed_f
void stateCalcAccelNed_f(void)
Definition: state.c:1133
orientationGetQuat_i
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
Definition: pprz_orientation_conversion.h:198
RATE_I
#define RATE_I
Definition: state.h:112
stateSetHorizontalWindspeed_f
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
State::h_speed_norm_f
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
LLA_FLOAT_OF_BFP
#define LLA_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:177
stateSetPositionUtm_f
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
pprz_algebra_float.h
Paparazzi floating point algebra.
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
POS_LLA_I
#define POS_LLA_I
Definition: state.h:70
stateCalcHorizontalWindspeed_i
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1248
stateCalcAirspeed_i
void stateCalcAirspeed_i(void)
Definition: state.c:1279
stateGetBodyRates_i
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
stateSetNedToBodyRMat_f
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
stateGetHorizontalSpeedDir_i
static int32_t stateGetHorizontalSpeedDir_i(void)
Get dir of horizontal ground speed (int).
Definition: state.h:899
stateSetSpeedEcef_f
static void stateSetSpeedEcef_f(struct EcefCoor_f *ecef_speed)
Set ground speed in ECEF coordinates (float).
Definition: state.h:825
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
stateCalcPositionNed_f
void stateCalcPositionNed_f(void)
Definition: state.c:433
State::windspeed_f
union State::@341 windspeed_f
Horizontal windspeed.
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
State::ned_speed_i
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
AIRSPEED_I
#define AIRSPEED_I
Definition: state.h:122
pprz_algebra_int.h
Paparazzi fixed point algebra.
LtpDef_f::lla
struct LlaCoor_f lla
origin of local frame in LLA
Definition: pprz_geodetic_float.h:95
SIDESLIP_F
#define SIDESLIP_F
Definition: state.h:127
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
SPEED_ENU_F
#define SPEED_ENU_F
Definition: state.h:92
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
State::enu_speed_i
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
stateGetAngleOfAttack_f
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
stateGetSpeedEcef_i
static struct EcefCoor_i * stateGetSpeedEcef_i(void)
Get ground speed in ECEF coordinates (int).
Definition: state.h:881
std.h
stateSetLocalUtmOrigin_f
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
stateSetBodyRates_i
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1173
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
stateSetPositionLla_f
static void stateSetPositionLla_f(struct LlaCoor_f *lla_pos)
Set position from LLA coordinates (float).
Definition: state.h:614
WINDSPEED_F
#define WINDSPEED_F
Definition: state.h:123
stateCalcPositionEnu_f
void stateCalcPositionEnu_f(void)
Definition: state.c:500
stateGetSpeedEnu_i
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
LtpDef_f::hmsl
float hmsl
Height above mean sea level in meters.
Definition: pprz_geodetic_float.h:97
stateSetAirspeed_i
static void stateSetAirspeed_i(int32_t airspeed)
Set airspeed (int).
Definition: state.h:1282
HIGH_RES_RMAT_FLOAT_OF_BFP
#define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_geodetic_int.h:223
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
stateGetWindspeed_f
static struct FloatVect3 * stateGetWindspeed_f(void)
Get windspeed (float).
Definition: state.h:1395
stateGetSpeedNed_i
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
LtpDef_f::ltp_of_ecef
struct FloatRMat ltp_of_ecef
rotation from ECEF to local frame
Definition: pprz_geodetic_float.h:96
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
stateSetVerticalWindspeed_f
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
ECEF_FLOAT_OF_BFP
#define ECEF_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:160
stateIsRateValid
static bool stateIsRateValid(void)
Test if rates are valid.
Definition: state.h:1165
State::utm_initialized_f
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
stateSetVerticalWindspeed_i
static void stateSetVerticalWindspeed_i(int32_t v_windspeed)
Set vertical windspeed (int).
Definition: state.h:1273
ACCEL_NED_F
#define ACCEL_NED_F
Definition: state.h:105
stateCalcPositionEcef_i
void stateCalcPositionEcef_i(void)
Definition: state.c:68
stateIsAttitudeValid
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
stateIsAirspeedValid
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1244
stateSetPositionEnu_f
static void stateSetPositionEnu_f(struct EnuCoor_f *enu_pos)
Set position from local ENU coordinates (float).
Definition: state.h:606
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Vect3
Definition: pprz_algebra_int.h:88
stateSetSpeedEnu_i
static void stateSetSpeedEnu_i(struct EnuCoor_i *enu_speed)
Set ground speed in local ENU coordinates (int).
Definition: state.h:771
uint8_t
unsigned char uint8_t
Definition: types.h:14
stateSetNedToBodyQuat_i
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:1075
stateSetNedToBodyEulers_i
static void stateSetNedToBodyEulers_i(struct Int32Eulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (int).
Definition: state.h:1087
stateGetSpeedEcef_f
static struct EcefCoor_f * stateGetSpeedEcef_f(void)
Get ground speed in ECEF coordinates (float).
Definition: state.h:926
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
stateCalcPositionEcef_f
void stateCalcPositionEcef_f(void)
Definition: state.c:402
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
POS_LOCAL_COORD
#define POS_LOCAL_COORD
Definition: state.h:77
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
stateIsWindspeedValid
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition: state.h:1232
State::lla_pos_i
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
stateIsSideslipValid
static bool stateIsSideslipValid(void)
test if sideslip is available.
Definition: state.h:1256
stateSetSpeed_i
static void stateSetSpeed_i(struct EcefCoor_i *ecef_speed, struct NedCoor_i *ned_speed, struct EnuCoor_i *enu_speed)
Set multiple speed coordinates (int).
Definition: state.h:787
stateCalcSpeedEcef_f
void stateCalcSpeedEcef_f(void)
Definition: state.c:978
stateSetPositionEnu_i
static void stateSetPositionEnu_i(struct EnuCoor_i *enu_pos)
Set position from local ENU coordinates (int).
Definition: state.h:539
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition: pprz_orientation_conversion.h:189
stateSetAngleOfAttack_f
static void stateSetAngleOfAttack_f(float aoa)
Set angle of attack in radians (float).
Definition: state.h:1318
SPEED_NED_F
#define SPEED_NED_F
Definition: state.h:91
stateSetAccelEcef_i
static void stateSetAccelEcef_i(struct EcefCoor_i *ecef_accel)
Set acceleration in ECEF coordinates (int).
Definition: state.h:994
ACCEL_ECEF_F
#define ACCEL_ECEF_F
Definition: state.h:104
State::body_rates_i
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
SPEED_LOCAL_COORD
#define SPEED_LOCAL_COORD
Definition: state.h:95
SPEED_HNORM_I
#define SPEED_HNORM_I
Definition: state.h:88
POS_ENU_I
#define POS_ENU_I
Definition: state.h:69
stateSetPositionNed_i
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
stateGetWindspeed_i
static struct Int32Vect3 * stateGetWindspeed_i(void)
Get windspeed (int).
Definition: state.h:1356
stateCalcAirspeed_f
void stateCalcAirspeed_f(void)
Definition: state.c:1325
POS_NED_F
#define POS_NED_F
Definition: state.h:73
stateGetNedToBodyRMat_f
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
stateSetPosition_i
static void stateSetPosition_i(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:555
State::enu_speed_f
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
stateCalcHorizontalSpeedNorm_f
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:1003
stateGetSideslip_f
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition: state.h:1425
stateGetHorizontalWindspeed_f
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
SPEED_HDIR_I
#define SPEED_HDIR_I
Definition: state.h:89
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
stateCalcAccelNed_i
void stateCalcAccelNed_i(void)
Definition: state.c:1068
SPEED_ECEF_I
#define SPEED_ECEF_I
Definition: state.h:85
stateGetAccelEcef_i
static struct EcefCoor_i * stateGetAccelEcef_i(void)
Get acceleration in ECEF coordinates (int).
Definition: state.h:1029
stateGetHorizontalWindspeed_i
static struct Int32Vect2 * stateGetHorizontalWindspeed_i(void)
Get horizontal windspeed (int).
Definition: state.h:1338
stateSetPosition_f
static void stateSetPosition_f(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:622
M_OF_MM
#define M_OF_MM(_mm)
Definition: pprz_geodetic_int.h:134
SPEED_ENU_I
#define SPEED_ENU_I
Definition: state.h:87
stateGetAirspeed_i
static int32_t stateGetAirspeed_i(void)
Get airspeed (int).
Definition: state.h:1368
stateCalcSpeedNed_i
void stateCalcSpeedNed_i(void)
Definition: state.c:674
State::ned_speed_f
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
stateCalcHorizontalSpeedDir_i
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:847
LtpDef_i::ltp_of_ecef
struct Int32RMat ltp_of_ecef
Rotation matrix.
Definition: pprz_geodetic_int.h:101
POS_GLOBAL_COORD
#define POS_GLOBAL_COORD
Definition: state.h:78
POS_NED_I
#define POS_NED_I
Definition: state.h:68
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
orientationSetRMat_i
static void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: pprz_orientation_conversion.h:157
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
State::windspeed_i
union State::@340 windspeed_i
Horizontal windspeed in north/east/down.
EcefCoor_f
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_float.h:45
stateGetAccelBody_i
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
State::ned_accel_i
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
stateGetPositionEcef_i
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition: state.h:656
State::body_accel_i
struct Int32Vect3 body_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:338
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
pprz_orientation_conversion.h
stateCalcVerticalWindspeed_f
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1310
State::h_speed_norm_i
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
State::ned_pos_i
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
State::ecef_speed_i
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
State::sideslip_f
float sideslip_f
Sideslip angle Unit: rad.
Definition: state.h:443
State::ecef_pos_f
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
stateIsGlobalCoordinateValid
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition: state.h:515
State
Structure holding vehicle state data.
Definition: state.h:134
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
State::enu_pos_i
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
orientationGetEulers_i
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
Definition: pprz_orientation_conversion.h:216
stateGetHorizontalSpeedNorm_i
static uint32_t stateGetHorizontalSpeedNorm_i(void)
Get norm of horizontal ground speed (int).
Definition: state.h:890
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
stateCalcVerticalWindspeed_i
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1264
SPEED_ECEF_F
#define SPEED_ECEF_F
Definition: state.h:90
stateSetPositionLla_i
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: state.h:547
DOWNWIND_I
#define DOWNWIND_I
Definition: state.h:121
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
stateCalcPositionEnu_i
void stateCalcPositionEnu_i(void)
Definition: state.c:180
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
orientationSetQuat_i
static void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
Set vehicle body attitude from quaternion (int).
Definition: pprz_orientation_conversion.h:149
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
orienationCheckValid
static bool orienationCheckValid(struct OrientationReps *orientation)
Test if orientations are valid.
Definition: pprz_orientation_conversion.h:135
State::alt_agl_f
float alt_agl_f
Altitude above ground level.
Definition: state.h:198
SPEED_HDIR_F
#define SPEED_HDIR_F
Definition: state.h:94
stateSetNedToBodyRMat_i
static void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: state.h:1081
State::body_rates_f
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
POS_UTM_F
#define POS_UTM_F
Definition: state.h:76
RATE_F
#define RATE_F
Definition: state.h:113
stateSetPositionEcef_f
static void stateSetPositionEcef_f(struct EcefCoor_f *ecef_pos)
Set position from ECEF coordinates (float).
Definition: state.h:590
stateSetSpeedEnu_f
static void stateSetSpeedEnu_f(struct EnuCoor_f *enu_speed)
Set ground speed in local ENU coordinates (float).
Definition: state.h:817
State::enu_pos_f
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
stateGetPositionLla_f
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
stateCalcPositionUtm_f
void stateCalcPositionUtm_f(void)
Definition: state.c:365
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
WINDSPEED_I
#define WINDSPEED_I
Definition: state.h:120
stateGetAccelEcef_f
static struct EcefCoor_f * stateGetAccelEcef_f(void)
Get acceleration in ECEF coordinates (float).
Definition: state.h:1047
stateSetHorizontalWindspeed_i
static void stateSetHorizontalWindspeed_i(struct Int32Vect2 *h_windspeed)
Set horizontal windspeed (int).
Definition: state.h:1264
SPEED_NED_I
#define SPEED_NED_I
Definition: state.h:86
POS_ENU_F
#define POS_ENU_F
Definition: state.h:74
state
struct State state
Definition: state.c:36
State::airspeed_i
int32_t airspeed_i
Norm of relative wind speed.
Definition: state.h:416
stateIsAngleOfAttackValid
static bool stateIsAngleOfAttackValid(void)
test if angle of attack is available.
Definition: state.h:1250
stateSetSpeedNed_i
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
POS_LLA_F
#define POS_LLA_F
Definition: state.h:75
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
State::utm_pos_f
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
AIRSPEED_F
#define AIRSPEED_F
Definition: state.h:125
State::wind_air_status
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
stateGetVerticalWindspeed_f
static float stateGetVerticalWindspeed_f(void)
Get vertical windspeed (float).
Definition: state.h:1386
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
POS_ECEF_I
#define POS_ECEF_I
Definition: state.h:67
stateCalcHorizontalSpeedDir_f
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1030
orientationGetEulers_f
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
Definition: pprz_orientation_conversion.h:243
stateSetSpeed_f
static void stateSetSpeed_f(struct EcefCoor_f *ecef_speed, struct NedCoor_f *ned_speed, struct EnuCoor_f *enu_speed)
Set multiple speed coordinates (float).
Definition: state.h:833
State::ecef_accel_i
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
State::ecef_speed_f
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
stateGetPositionEcef_f
static struct EcefCoor_f * stateGetPositionEcef_f(void)
Get position in ECEF coordinates (float).
Definition: state.h:701
State::ned_pos_f
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:243
AOA_F
#define AOA_F
Definition: state.h:126
LtpDef_f::ecef
struct EcefCoor_f ecef
origin of local frame in ECEF
Definition: pprz_geodetic_float.h:94
stateGetVerticalWindspeed_i
static float stateGetVerticalWindspeed_i(void)
Get vertical windspeed (int).
Definition: state.h:1347
stateSetAccelEcef_f
static void stateSetAccelEcef_f(struct EcefCoor_f *ecef_accel)
Set acceleration in ECEF coordinates (float).
Definition: state.h:1010
LlaCoor_f
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_float.h:54
stateSetSideslip_f
static void stateSetSideslip_f(float sideslip)
Set sideslip angle in radians (float).
Definition: state.h:1327