DevGuide/StateInterface

The idea is to create a general state interface that holds the most important vehicle states like attitude, position, speed, acceleration.

See the new-state-interface branch on github for the current status.

state.h

/*** functions to set state (int versions) ***/
inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos);
inline void StateSetPositionNed_i(struct NedCoor_i* ned_pos);
inline void StateSetPositionLla_i(struct LlaCoor_i* lla_pos);

inline void StateSetSpeedNed_i(struct NedCoor_i* ned_speed);
inline void StateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed);
inline void StateSetAccelNed_i(struct NedCoor_i* ned_accel);
inline void StateSetAccelEcef_i(struct EcefCoor_i* ecef_accel);

inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat);
inline void StateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat);
inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers);
inline void StateSetBodyRates_i(struct Int32Rates* body_rate);

inline void StateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed);
inline void StateSetAirspeed_i(int32_t* airspeed);


/****************************************
 * functions to get state (int versions)
 ****************************************/
inline struct EcefCoor_i StateGetPositionEcef_i(void);
inline struct NedCoor_i  StateGetPositionNed_i(void);
inline struct LlaCoor_i  StateGetPositionLla_i(void);

inline struct NedCoor_i StateGetSpeedNed_i(void);
inline struct EcefCoor_i StateGetSpeedEcef_i(void);
inline int32_t StateGetHorizontalSpeedNorm_i(void);
inline int32_t StateGetHorizontalSpeedDir_i(void);

inline struct NedCoor_i StateGetAccelNed_i(void);
inline struct EcefCoor_i StateGetAccelEcef_i(void);

inline struct Int32Quat   StateGetNedToBodyQuat_i(void);
inline struct Int32RMat   StateGetNedToBodyRMat_i(void);
inline struct Int32Eulers StateGetNedToBodyEulers_i(void);
inline struct Int32Rates  StateGetBodyRates_i(void);

inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void);
inline int32_t StateGetAirspeed_i(void);


/*** functions to set state (float versions) ***/
inline void StateSetPositionUtm_f(struct FloatVect3* utm_pos);
inline void StateSetPositionEcef_f(struct EcefCoor_f* ecef_pos);
inline void StateSetPositionNed_f(struct NedCoor_f* ned_pos);
inline void StateSetPositionLla_f(struct LlaCoor_f* lla_pos);

inline void StateSetSpeedNed_f(struct NedCoor_f* ned_speed);
inline void StateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed);
//inline void StateSetHorizontalSpeedNorm_f(float* h_speed_norm);
//inline void StateSetHorizontalSpeedDirection_f(float* h_speed_dir);

inline void StateSetAccelNed_f(struct NedCoor_f* ned_accel);
inline void StateSetAccelEcef_f(struct EcefCoor_f* ecef_accel);

inline void StateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat);
inline void StateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat);
inline void StateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers);
inline void StateSetBodyRates_f(struct FloatRates* body_rate);

inline void StateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed);
inline void StateSetAirspeed_f(float* airspeed);

/*** functions to get state (float versions) ***/
inline struct FloatVect3 StateGetPositionUtm_f(void);
inline struct EcefCoor_f StateGetPositionEcef_f(void);
inline struct NedCoor_f  StateGetPositionNed_f(void);
inline struct LlaCoor_f  StateGetPositionLla_f(void);

inline struct NedCoor_f StateGetSpeedNed_f(void);
inline struct EcefCoor_f StateGetSpeedEcef_f(void);
inline float StateGetHorizontalSpeedNorm_f(void);
inline float StateGetHorizontalSpeedDir_f(void);

inline struct NedCoor_f StateGetAccelNed_f(void);
inline struct EcefCoor_f StateGetAccelEcef_f(void);

inline struct FloatQuat   StateGetNedToBodyQuat_f(void);
inline struct FloatRMat   StateGetNedToBodyRMat_f(void);
inline struct FloatEulers StateGetNedToBodyEulers_f(void);
inline struct FloatRates  StateGetBodyRates_f(void);

inline struct FloatVect2 StateGetHorizontalWindspeed_f(void);
inline float StateGetAirspeed_f(void);

We should also make clear that the angle psi means the heading (where the aircraft's noise is pointing) and not the direction it is actually moving (groundspeed direction). For fixedwings I think we currently assume it is the same (without a magnetometer).

On time computation

With this solution, the state can be updated and read with any format with a little overhead (do some tests on a status).

The conversion is done only if needed and is not done again until the state is updated.

The following structure has to be done for the different "groups" of states: position, speed, attitude,...

#define POS_ECEF_I 1<<0
#define POS_NED_I  1<<1
#define POS_LLA_I  1<<2
#define POS_UTM_I  1<<3
#define POS_ECEF_F 1<<4
#define POS_NED_F  1<<5
#define POS_LLA_F  1<<6
#define POS_UTM_F  1<<7


// set the new value and reset the status to tell that other format are not up to date anymore
inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) {
  INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos);
  /* clear bits for all position representations and only set the new one */
  state.pos_status = (1 << POS_ECEF_I);
}

--> do the same for YYY and ZZZ, and even for XXX_YYY, XXX_ZZZ, ... if needed

// get the value and transformation if needed
inline struct NedCoor_i StateGetPositionNed_i(void) {
  if (!bit_is_set(state.pos_status, POS_NED_I)) {
    if (state.ned_initialised_i) {
      if (bit_is_set(state.pos_status, POS_NED_F)) {
        NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
      } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
        ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
        struct NedCoor_f ned_f;
        ned_of_lla_point_f(&ned_f, &state.ned_origin_f, &state.lla_pos_f);
        NED_BFP_OF_REAL(state.ned_pos_i, ned_f);
      } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
        ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
      } else {
        /* could not get this representation,  set errno */
        struct NedCoor_i ned_i;
        INT32_VECT3_ZERO(ned_i);
        return ned_i;
      }
    } else {
      /* ned coordinate system not initialized,  set errno */
      struct NedCoor_i ned;
      INT32_VECT3_ZERO(ned);
      return ned;
    }
    /* set bit to indicate this representation is computed */
    SetBit(state.pos_status, POS_NED_I);
  }
  return state.ned_pos_i;
}

--> do the same for the other states