Paparazzi UAS  v7.0_unstable
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))
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))
102 #define ACCEL_ECEF_I 0
103 #define ACCEL_NED_I 1
104 #define ACCEL_ECEF_F 2
105 #define ACCEL_NED_F 3
112 #define RATE_I 0
113 #define RATE_F 1
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
134 struct State {
135 
145 
150  struct EcefCoor_i ecef_pos_i;
151 
157  struct LlaCoor_i lla_pos_i;
158 
166  struct LtpDef_i ned_origin_i;
167 
172 
178  struct NedCoor_i ned_pos_i;
179 
185  struct EnuCoor_i enu_pos_i;
186 
192  struct UtmCoor_f utm_pos_f;
193 
198  float alt_agl_f;
199 
205  struct LlaCoor_f lla_pos_f;
206 
211  struct EcefCoor_f ecef_pos_f;
212 
220  struct LtpDef_f ned_origin_f;
221 
224 
233  struct UtmCoor_f utm_origin_f;
234 
237 
243  struct NedCoor_f ned_pos_f;
244 
250  struct EnuCoor_f enu_pos_f;
262 
267  struct EcefCoor_i ecef_speed_i;
268 
273  struct NedCoor_i ned_speed_i;
274 
279  struct EnuCoor_i enu_speed_i;
280 
286 
293 
298  struct EcefCoor_f ecef_speed_f;
299 
303  struct NedCoor_f ned_speed_f;
304 
309  struct EnuCoor_f enu_speed_f;
310 
316 
333 
338  struct Int32Vect3 body_accel_i;
339 
344  struct NedCoor_i ned_accel_i;
345 
350  struct EcefCoor_i ecef_accel_i;
351 
356  struct NedCoor_f ned_accel_f;
357 
362  struct EcefCoor_f ecef_accel_f;
369 
370 
379 
384  struct Int32Rates body_rates_i;
385 
390  struct FloatRates body_rates_f;
402 
407  union {
408  struct Int32Vect3 vect3;
409  struct Int32Vect2 vect2;
411 
417 
422  union {
423  struct FloatVect3 vect3;
424  struct FloatVect2 vect2;
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 
457 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 */
euler angles
Roation quaternion.
rotation matrix
angular rates
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
euler angles
Rotation quaternion.
rotation matrix
angular rates
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:58
struct Int32RMat ltp_of_ecef
Rotation matrix.
int32_t hmsl
Height above mean sea level in mm.
struct LlaCoor_i lla
Reference point in lla.
struct EcefCoor_i ecef
Reference point in ecef.
#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 FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (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 Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
static void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
Set vehicle body attitude from quaternion (int).
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat)
Set vehicle body attitude from rotation matrix (int).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
#define ACCEL_ECEF_I
Definition: state.h:102
struct EcefCoor_f ecef_accel_f
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:362
struct Int32Vect3 body_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:338
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
struct NedCoor_i ned_accel_i
Acceleration in North East Down coordinates.
Definition: state.h:344
#define ACCEL_NED_F
Definition: state.h:105
static struct EcefCoor_f * stateGetAccelEcef_f(void)
Get acceleration in ECEF coordinates (float).
Definition: state.h:1047
uint8_t accel_status
Holds the status bits for all acceleration representations.
Definition: state.h:332
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
void stateCalcAccelNed_i(void)
Definition: state.c:1087
void stateCalcAccelEcef_i(void)
Definition: state.c:1120
struct EcefCoor_i ecef_accel_i
Acceleration in EarthCenteredEarthFixed coordinates.
Definition: state.h:350
static bool stateIsAccelValid(void)
Test if accelerations are valid.
Definition: state.h:978
#define ACCEL_ECEF_F
Definition: state.h:104
void stateCalcAccelEcef_f(void)
Definition: state.c:1185
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
static void stateSetAccelEcef_f(struct EcefCoor_f *ecef_accel)
Set acceleration in ECEF coordinates (float).
Definition: state.h:1010
static void stateSetAccelEcef_i(struct EcefCoor_i *ecef_accel)
Set acceleration in ECEF coordinates (int).
Definition: state.h:994
static struct EcefCoor_i * stateGetAccelEcef_i(void)
Get acceleration in ECEF coordinates (int).
Definition: state.h:1029
struct NedCoor_f ned_accel_f
Acceleration in North East Down coordinates.
Definition: state.h:356
#define ACCEL_NED_I
Definition: state.h:103
void stateCalcAccelNed_f(void)
Definition: state.c:1152
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
static void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: state.h:1081
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:1075
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
static void stateSetNedToBodyEulers_i(struct Int32Eulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (int).
Definition: state.h:1087
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
struct OrientationReps ned_to_body_orientation
Definition: state.h:368
struct State state
Definition: state.c:36
void stateInit(void)
Definition: state.c:57
Structure holding vehicle state data.
Definition: state.h:134
#define POS_LLA_F
Definition: state.h:75
struct NedCoor_i ned_pos_i
Position in North East Down coordinates.
Definition: state.h:178
#define POS_UTM_F
Definition: state.h:76
static void stateSetPositionEcef_f(struct EcefCoor_f *ecef_pos)
Set position from ECEF coordinates (float).
Definition: state.h:590
#define POS_LLA_I
Definition: state.h:70
struct NedCoor_f ned_pos_f
Position in North East Down coordinates.
Definition: state.h:243
#define POS_ENU_F
Definition: state.h:74
struct EnuCoor_f enu_pos_f
Position in East North Up coordinates.
Definition: state.h:250
#define POS_ECEF_F
Definition: state.h:72
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
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: state.h:192
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
void stateCalcPositionNed_f(void)
Definition: state.c:452
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
uint16_t pos_status
Holds the status bits for all position representations.
Definition: state.h:144
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: state.h:157
void stateCalcPositionUtm_f(void)
Definition: state.c:384
void stateCalcPositionEnu_f(void)
Definition: state.c:519
struct EcefCoor_f ecef_pos_f
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:211
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static void stateSetPositionEnu_f(struct EnuCoor_f *enu_pos)
Set position from local ENU coordinates (float).
Definition: state.h:606
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition: state.h:656
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
#define POS_ENU_I
Definition: state.h:69
#define POS_ECEF_I
Definition: state.h:67
static void stateSetPositionEcef_i(struct EcefCoor_i *ecef_pos)
Set position from ECEF coordinates (int).
Definition: state.h:523
#define POS_GLOBAL_COORD
Definition: state.h:78
void stateCalcPositionEcef_f(void)
Definition: state.c:421
#define POS_LOCAL_COORD
Definition: state.h:77
static void stateSetPositionEnu_i(struct EnuCoor_i *enu_pos)
Set position from local ENU coordinates (int).
Definition: state.h:539
struct EnuCoor_i enu_pos_i
Position in East North Up coordinates.
Definition: state.h:185
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static void stateSetPositionLla_f(struct LlaCoor_f *lla_pos)
Set position from LLA coordinates (float).
Definition: state.h:614
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
#define POS_NED_F
Definition: state.h:73
static bool stateIsGlobalCoordinateValid(void)
Test if global coordinates are valid.
Definition: state.h:515
float alt_agl_f
Altitude above ground level.
Definition: state.h:198
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: state.h:547
void stateCalcPositionLla_f(void)
Definition: state.c:592
void stateCalcPositionEcef_i(void)
Definition: state.c:87
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
static struct EcefCoor_f * stateGetPositionEcef_f(void)
Get position in ECEF coordinates (float).
Definition: state.h:701
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
void stateCalcPositionNed_i(void)
Definition: state.c:119
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
void stateCalcPositionLla_i(void)
Calculate LLA (int) from any other available representation.
Definition: state.c:286
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: state.h:205
struct EcefCoor_i ecef_pos_i
Position in EarthCenteredEarthFixed coordinates.
Definition: state.h:150
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define POS_NED_I
Definition: state.h:68
void stateCalcPositionEnu_i(void)
Definition: state.c:199
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
void stateCalcBodyRates_f(void)
Definition: state.c:1241
#define RATE_F
Definition: state.h:113
struct FloatRates body_rates_f
Angular rates in body frame.
Definition: state.h:390
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
#define RATE_I
Definition: state.h:112
void stateCalcBodyRates_i(void)
Definition: state.c:1226
struct Int32Rates body_rates_i
Angular rates in body frame.
Definition: state.h:384
uint8_t rate_status
Holds the status bits for all angular rate representations.
Definition: state.h:378
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1173
static bool stateIsRateValid(void)
Test if rates are valid.
Definition: state.h:1165
#define SPEED_ENU_F
Definition: state.h:92
static void stateSetSpeedEcef_i(struct EcefCoor_i *ecef_speed)
Set ground speed in ECEF coordinates (int).
Definition: state.h:779
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
#define SPEED_ENU_I
Definition: state.h:87
void stateCalcSpeedEnu_f(void)
Definition: state.c:947
struct NedCoor_f ned_speed_f
speed in North East Down coordinates
Definition: state.h:303
#define SPEED_ECEF_F
Definition: state.h:90
void stateCalcSpeedNed_f(void)
Definition: state.c:897
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
#define SPEED_ECEF_I
Definition: state.h:85
void stateCalcHorizontalSpeedDir_i(void)
Definition: state.c:866
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static void stateSetSpeedEcef_f(struct EcefCoor_f *ecef_speed)
Set ground speed in ECEF coordinates (float).
Definition: state.h:825
#define SPEED_NED_F
Definition: state.h:91
struct NedCoor_i ned_speed_i
Velocity in North East Down coordinates.
Definition: state.h:273
float h_speed_norm_f
Norm of horizontal ground speed.
Definition: state.h:315
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
int32_t h_speed_dir_i
Direction of horizontal ground speed.
Definition: state.h:292
float h_speed_dir_f
Direction of horizontal ground speed.
Definition: state.h:321
void stateCalcHorizontalSpeedNorm_f(void)
Definition: state.c:1022
void stateCalcHorizontalSpeedNorm_i(void)
Definition: state.c:820
#define SPEED_LOCAL_COORD
Definition: state.h:95
static int32_t stateGetHorizontalSpeedDir_i(void)
Get dir of horizontal ground speed (int).
Definition: state.h:899
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
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
struct EcefCoor_f ecef_speed_f
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:298
void stateCalcSpeedEcef_f(void)
Definition: state.c:997
#define SPEED_HDIR_I
Definition: state.h:89
void stateCalcSpeedEnu_i(void)
Definition: state.c:743
static void stateSetSpeedEnu_f(struct EnuCoor_f *enu_speed)
Set ground speed in local ENU coordinates (float).
Definition: state.h:817
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
#define SPEED_HNORM_F
Definition: state.h:93
#define SPEED_NED_I
Definition: state.h:86
struct EcefCoor_i ecef_speed_i
Velocity in EarthCenteredEarthFixed coordinates.
Definition: state.h:267
static void stateSetSpeedEnu_i(struct EnuCoor_i *enu_speed)
Set ground speed in local ENU coordinates (int).
Definition: state.h:771
static struct EcefCoor_i * stateGetSpeedEcef_i(void)
Get ground speed in ECEF coordinates (int).
Definition: state.h:881
void stateCalcHorizontalSpeedDir_f(void)
Definition: state.c:1049
struct EnuCoor_i enu_speed_i
Velocity in East North Up coordinates.
Definition: state.h:279
uint16_t speed_status
Holds the status bits for all ground speed representations.
Definition: state.h:261
static struct EcefCoor_f * stateGetSpeedEcef_f(void)
Get ground speed in ECEF coordinates (float).
Definition: state.h:926
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
void stateCalcSpeedNed_i(void)
Definition: state.c:693
#define SPEED_HNORM_I
Definition: state.h:88
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
uint32_t h_speed_norm_i
Norm of horizontal ground speed.
Definition: state.h:285
static uint32_t stateGetHorizontalSpeedNorm_i(void)
Get norm of horizontal ground speed (int).
Definition: state.h:890
#define SPEED_HDIR_F
Definition: state.h:94
struct EnuCoor_f enu_speed_f
Velocity in East North Up coordinates.
Definition: state.h:309
void stateCalcSpeedEcef_i(void)
Definition: state.c:795
static bool stateIsSideslipValid(void)
test if sideslip is available.
Definition: state.h:1256
#define WINDSPEED_I
Definition: state.h:120
static struct Int32Vect3 * stateGetWindspeed_i(void)
Get windspeed (int).
Definition: state.h:1356
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition: state.h:1232
static void stateSetAirspeed_i(int32_t airspeed)
Set airspeed (int).
Definition: state.h:1282
int32_t airspeed_i
Norm of relative wind speed.
Definition: state.h:416
#define DOWNWIND_F
Definition: state.h:124
static void stateSetAngleOfAttack_f(float aoa)
Set angle of attack in radians (float).
Definition: state.h:1318
static void stateSetVerticalWindspeed_i(int32_t v_windspeed)
Set vertical windspeed (int).
Definition: state.h:1273
static struct Int32Vect2 * stateGetHorizontalWindspeed_i(void)
Get horizontal windspeed (int).
Definition: state.h:1338
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
static void stateSetHorizontalWindspeed_i(struct Int32Vect2 *h_windspeed)
Set horizontal windspeed (int).
Definition: state.h:1264
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition: state.h:1425
#define WINDSPEED_F
Definition: state.h:123
float airspeed_f
Norm of relative air speed.
Definition: state.h:431
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1244
uint8_t wind_air_status
Holds the status bits for all wind- and airspeed representations.
Definition: state.h:401
union State::@344 windspeed_f
Horizontal windspeed.
#define AIRSPEED_I
Definition: state.h:122
static void stateSetSideslip_f(float sideslip)
Set sideslip angle in radians (float).
Definition: state.h:1327
#define SIDESLIP_F
Definition: state.h:127
union State::@343 windspeed_i
Horizontal windspeed in north/east/down.
void stateCalcAirspeed_i(void)
Definition: state.c:1298
void stateCalcVerticalWindspeed_i(void)
Definition: state.c:1283
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static float stateGetVerticalWindspeed_i(void)
Get vertical windspeed (int).
Definition: state.h:1347
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
void stateCalcAirspeed_f(void)
Definition: state.c:1344
#define AIRSPEED_F
Definition: state.h:125
float sideslip_f
Sideslip angle Unit: rad.
Definition: state.h:443
void stateCalcHorizontalWindspeed_f(void)
Definition: state.c:1313
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
static float stateGetVerticalWindspeed_f(void)
Get vertical windspeed (float).
Definition: state.h:1386
static int32_t stateGetAirspeed_i(void)
Get airspeed (int).
Definition: state.h:1368
void stateCalcHorizontalWindspeed_i(void)
Definition: state.c:1267
float angle_of_attack_f
Angle of attack Unit: rad.
Definition: state.h:437
#define AOA_F
Definition: state.h:126
static bool stateIsAngleOfAttackValid(void)
test if angle of attack is available.
Definition: state.h:1250
#define DOWNWIND_I
Definition: state.h:121
void stateCalcVerticalWindspeed_f(void)
Definition: state.c:1329
static struct FloatVect3 * stateGetWindspeed_f(void)
Get windspeed (float).
Definition: state.h:1395
static bool stateIsVerticalWindspeedValid(void)
test if vertical wind speed is available.
Definition: state.h:1238
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Paparazzi floating point math for geodetic calculations.
float hmsl
Height above mean sea level in meters.
struct FloatRMat ltp_of_ecef
rotation from ECEF to local frame
struct EcefCoor_f ecef
origin of local frame in ECEF
struct LlaCoor_f lla
origin of local frame in LLA
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.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98