added some more stuff to new state interface

sw/airborne/state.c

@@ -49,11 +49,15 @@ inline void StateSetPositionLla_i(struct LlaCoor_i* lla_pos) {
 }
 
 inline void StateSetSpeedNed_i(struct NedCoor_i* ned_speed) {
-
+  INT32_VECT3_COPY(state.ned_speed_i, *ned_speed);
+  /* clear bits for all speed representations and only set the new one */
+  state.speed_status = (1 << SPEED_NED_I);
 }
 
 inline void StateSetAccelNed_i(struct NedCoor_i* ned_accel) {
-
+  INT32_VECT3_COPY(state.ned_accel_i, *ned_accel);
+  /* clear bits for all speed representations and only set the new one */
+  state.accel_status = (1 << ACCEL_NED_I);
 }
 
 inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat) {
@@ -75,7 +79,9 @@ inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers){
 }
 
 inline void StateSetBodyRates_i(struct Int32Rates* body_rate){
-
+  RATES_COPY(state.body_rates_i, *body_rate);
+  /* clear bits for all attitude representations and only set the new one */
+  state.rate_status = (1 << RATE_I);
 }
 
 
@@ -135,15 +141,35 @@ inline struct LlaCoor_i StateGetPositionLla_i(void) {
   return state.lla_pos_i;
 }
 
-/*
-  inline struct NedCoor_i StateGetSpeedNed_i(void) {
-
+inline struct NedCoor_i StateGetSpeedNed_i(void) {
+  if (!bit_is_set(state.speed_status, SPEED_NED_I)) {
+    if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
+      if (state.ned_initialised_i) {
+        ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
+      }
+    } else {
+      //try floats....
+    }
+    /* set bit to indicate this representation is computed */
+    SetBit(state.speed_status, SPEED_NED_I);
   }
+  return state.ned_speed_i;
+}
 
-  inline struct NedCoor_i StateGetAccelNed_i(void) {
-
+inline struct NedCoor_i StateGetAccelNed_i(void) {
+  if (!bit_is_set(state.accel_status, ACCEL_NED_I)) {
+    if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+      if (state.ned_initialised_i) {
+        ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
+      }
+    } else {
+      //try floats....
+    }
+    /* set bit to indicate this representation is computed */
+    SetBit(state.accel_status, ACCEL_NED_I);
   }
-*/
+  return state.ned_accel_i;
+}
 
 inline struct Int32Quat StateGetNedToBodyQuat_i(void) {
   if (!bit_is_set(state.att_status, ATT_QUAT_I)) {
@@ -190,8 +216,15 @@ inline struct Int32Eulers StateGetNedToBodyEulers_i(void) {
   return state.ned_to_body_eulers_i;
 }
 
-/*
-  inline struct Int32Rates StateGetBodyRates_i(void) {
 
+inline struct Int32Rates StateGetBodyRates_i(void) {
+  if (!bit_is_set(state.rate_status, RATE_I)) {
+    if (bit_is_set(state.rate_status, RATE_F)) {
+      RATES_FLOAT_OF_BFP(state.body_rates_i, state.body_rates_f);
+    }
+    /* set bit to indicate this representation is computed */
+    SetBit(state.rate_status, RATE_I);
   }
-*/
+  return state.body_rates_i;
+}
+

sw/airborne/state.h

@@ -43,13 +43,35 @@
 #define POS_LLA_F  1<<6
 #define POS_UTM_F  1<<7
 
+#define SPEED_ECEF_I  1<<0
+#define SPEED_NED_I   1<<1
+#define SPEED_HNORM_I 1<<2
+#define SPEED_HDIR_I  1<<3
+#define SPEED_ECEF_F  1<<4
+#define SPEED_NED_F   1<<5
+#define SPEED_HNORM_F 1<<6
+#define SPEED_HDIR_F  1<<7
+
+#define ACCEL_ECEF_I 1<<0
+#define ACCEL_NED_I  1<<1
+#define ACCEL_ECEF_F 1<<3
+#define ACCEL_NED_F  1<<4
+
 #define ATT_QUAT_I  1<<0
 #define ATT_EULER_I 1<<1
 #define ATT_RMAT_I  1<<2
 #define ATT_QUAT_F  1<<3
 #define ATT_EULER_F 1<<4
 #define ATT_RMAT_F  1<<5
 
+#define RATE_I 1<<0
+#define RATE_F 1<<1
+
+#define WINDSPEED_I 1<<0
+#define AIRSPEED_I  1<<1
+#define WINDSPEED_F 1<<2
+#define AIRSPEED_F  1<<3
+
 
 /* abstract state interface */
 struct State {
@@ -58,6 +80,8 @@ struct State {
 
   /* Earth Centered Earth Fixed in centimeters */
   struct EcefCoor_i ecef_pos_i;
+  struct EcefCoor_i ecef_speed_i;
+  struct EcefCoor_i ecef_accel_i;
 
   /* lon, lat in radians*1e7  */
   /* alt in centimeters above MSL  */
@@ -72,17 +96,23 @@ struct State {
   struct NedCoor_i ned_speed_i;
   struct NedCoor_i ned_accel_i;
 
-  /* vehicle attitude */
-  struct Int32Quat   ned_to_body_quat_i;
-  struct Int32Eulers ned_to_body_eulers_i;
-  struct Int32RMat   ned_to_body_rmat_i;
-  struct Int32Rates  body_rate_i;
+  /* horizontal ground speed in norm and dir (m/s, rad (CW/North))
+   * with SPEED_FRAC and ANGLE_FRAC
+   */
+  int32_t h_speed_norm_i;
+  int32_t h_speed_dir_i;
 
   /* horizontal windspeed x = north, y = east */
   struct Int32Vect2 h_windspeed_i;
 
+  /* airspeed norm in m/s with SPEED_FRAC */
   int32_t airspeed_i;
 
+  /* vehicle attitude */
+  struct Int32Quat   ned_to_body_quat_i;
+  struct Int32Eulers ned_to_body_eulers_i;
+  struct Int32RMat   ned_to_body_rmat_i;
+  struct Int32Rates  body_rates_i;
 
   /****** float representations ******/
 
@@ -108,12 +138,16 @@ struct State {
   struct FloatQuat   ned_to_body_quat_f;
   struct FloatEulers ned_to_body_eulers_f;
   struct FloatRMat   ned_to_body_rmat_f;
-  struct FloatRates  body_rate_f;
+  struct FloatRates  body_rates_f;
 
 
   /********** one time computation bookkeeping ********/
   uint8_t pos_status;
+  uint8_t speed_status;
+  uint8_t accel_status;
   uint8_t att_status;
+  uint8_t rate_status;
+  uint8_t wind_air_status;
 
 };
 

sw/airborne/subsystems/ahrs.h

@@ -89,6 +89,12 @@ extern float ahrs_mag_offset;
     RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate);                   \
   }
 
+/* copy attitude to state interface */
+#define AHRS_BODY_TO_STATE() {                          \
+    StateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat);    \
+    StateSetBodyRates_i(&ahrs.body_rate);               \
+  }
+
 extern void ahrs_init(void);
 extern void ahrs_align(void);
 extern void ahrs_propagate(void);

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c

@@ -38,7 +38,6 @@ static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_
 static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag);
 static inline void compute_imu_quat_and_rmat_from_euler(void);
 static inline void compute_body_orientation(void);
-static inline void set_state_attitude(void);
 
 #define F_UPDATE 512
 
@@ -221,8 +220,3 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
 
 }
 
-/* copy attitude to state interface */
-__attribute__ ((always_inline)) static inline void set_state_attitude(void) {
-  StateSetNedToBodyQuat_i(&ahrs.ltp_to_body_quat);
-  //StateSetBodyRates_i(&ahrs.body_rate);
-}