Difference between revisions of "DevGuide/StateInterface"

From PaparazziUAV
Jump to navigation Jump to search
(update)
Line 1: Line 1:
The idea is to create a general state interface that holds the most important vehicle states like attitude, position, speed, acceleration.
The idea is to create a general state interface that holds the most important vehicle states like attitude, position, speed, acceleration.
See the [https://github.com/paparazzi/paparazzi/tree/new-state-interface| new_state_interface branch on github] for the current status.


=== state.h ===
=== state.h ===
<pre>
<pre>
/*** 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 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);


/* abstract state interface */
inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat);
struct State {
inline void StateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat);
  /* Earth Centered Earth Fixed in centimeters */
inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers);
  struct EcefCoor_i ecef_pos;
inline void StateSetBodyRates_i(struct Int32Rates* body_rate);


  /* lon, lat in radians*1e7  */
inline void StateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed);
  /* alt in centimeters above MSL  */
inline void StateSetAirspeed_i(int32_t* airspeed);
  struct LlaCoor_i lla_pos;


  /* definition of the local (flat earth) coordinate system */
  struct LtpDef_i ned_origin;
  bool_t ned_initialised;


  /* North East Down local tangent plane */
/****************************************
  struct NedCoor_i ned_pos;
* functions to get state (int versions)
  struct NedCoor_i ned_speed;
****************************************/
  struct NedCoor_i ned_accel;
inline struct EcefCoor_i StateGetPositionEcef_i(void);
inline struct NedCoor_i StateGetPositionNed_i(void);
inline struct LlaCoor_i  StateGetPositionLla_i(void);


  /* vehicle attitude */
inline struct NedCoor_i StateGetSpeedNed_i(void);
  struct Int32Quat  ned_to_body_quat;
inline struct EcefCoor_i StateGetSpeedEcef_i(void);
  struct Int32Eulers ned_to_body_euler;
inline int32_t StateGetHorizontalSpeedNorm_i(void);
  struct Int32RMat  ned_to_body_rmat;
inline int32_t StateGetHorizontalSpeedDir_i(void);
  struct Int32Rates  body_rate;


  /* horizontal windspeed x = north, y = east */
inline struct NedCoor_i StateGetAccelNed_i(void);
  struct Int32Vect2 h_windspeed;
inline struct EcefCoor_i StateGetAccelEcef_i(void);


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


  /***** float representations ****/
inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void);
inline int32_t StateGetAirspeed_i(void);


  /* Position within UTM zone in meters, z in meters above MSL */
  struct FloatVect3 utm_pos;
  uint8_t utm_zone;
  /* altitude above ground level in meters */
  float alt_agl;


  /* accelerations in North East Down local tangent plane */
/*** functions to set state (float versions) ***/
  struct FloatVect3 ned_accel;
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);


  /* speed in North East Down local tangent plane */
inline void StateSetSpeedNed_f(struct NedCoor_f* ned_speed);
  struct FloatVect3 ned_speed;
inline void StateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed);
  /* horizontal ground speed in norm and dir (m/s, rad (CW/North)) */
//inline void StateSetHorizontalSpeedNorm_f(float* h_speed_norm);
  float h_speed_norm;
//inline void StateSetHorizontalSpeedDirection_f(float* h_speed_dir);
  float h_speed_dir;


  struct FloatVect2 h_windspeed; /* m/s ; x = north, y = east */
inline void StateSetAccelNed_f(struct NedCoor_f* ned_accel);
  float airspeed_norm; /* m/s */
inline void StateSetAccelEcef_f(struct EcefCoor_f* ecef_accel);


  struct FloatQuat   ned_to_body_quat;
inline void StateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat);
  struct FloatEulers ned_to_body_euler;
inline void StateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat);
  struct FloatRMat  ned_to_body_rmat;
inline void StateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers);
  struct FloatRates body_rate;
inline void StateSetBodyRates_f(struct FloatRates* body_rate);
}
 
inline void StateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed);
inline void StateSetAirspeed_f(float* airspeed);


</pre>
/*** 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);


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).
inline struct NedCoor_f StateGetAccelNed_f(void);
inline struct EcefCoor_f StateGetAccelEcef_f(void);


=== How to set/get the state ? ===
inline struct FloatQuat  StateGetNedToBodyQuat_f(void);
inline struct FloatRMat  StateGetNedToBodyRMat_f(void);
inline struct FloatEulers StateGetNedToBodyEulers_f(void);
inline struct FloatRates  StateGetBodyRates_f(void);


State accessor functions we need:
inline struct FloatVect2 StateGetHorizontalWindspeed_f(void);
Integer set versions (inline void):
inline float StateGetAirspeed_f(void);
* StateSetPositionEcef_i(EcefCoor_i ecef_pos)
* StateSetPositionNed_i(NedCoor_i ned_pos)
* StateSetPositionLla_i(LlaCoor_i lla_pos)


* StateSetSpeedNed_i(NedCoor_i ned_speed)
</pre>
* StateSetAccelNed_i(NedCoor_i ned_accel)


* StateSetNedToBodyQuat_i(Int32Quat ned_to_body_quat)
* StateSetNedToBodyRMat_i(Int32RMat ned_to_body_rmat)
* StateSetNedToBodyEulers_i(Int32Eulers ned_to_body_eulers)
* StateSetBodyRates_i(Int32Rates body_rate)


* StateSetHorizontalWindspeed(Int32Vect2 h_windspeed)
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).
* StateSetAirspeedNorm(int32_t airspeed_norm)


== On time computation ==
== On time computation ==
Line 92: Line 170:


<pre>
<pre>
#define XXX 1<<0
#define POS_ECEF_I 1<<0
#define YYY 1<<1
#define POS_NED_I  1<<1
#define ZZZ 1<<2
#define POS_LLA_I  1<<2
uint XYZ_status;
#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
// set the new value and reset the status to tell that other format are not up to date anymore
#define STATE_SET_XXX(_v) {
inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) {
state.XXX = _v;
  INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos);
pos_status = XXX;
  /* 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
--> do the same for YYY and ZZZ, and even for XXX_YYY, XXX_ZZZ, ... if needed


// get the value and transformation if needed
// get the value and transformation if needed
#define STATE_GET_XXX(_r) {
inline struct NedCoor_i StateGetPositionNed_i(void) {
if (status bit XXX is not set) {
  if (!bit_is_set(state.pos_status, POS_NED_I)) {
  transform_XXX();
    if (state.ned_initialised_i) {
  set bit XXX in status;
      if (bit_is_set(state.pos_status, POS_NED_F)) {
}
        NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
_r = XXX;
      } 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


// conversion function
// the order the "if _ else if _" is chosen to begin with the less cpu consuming conversions
void transform_XXX() {
if (status bit YYY is set) {
  compute XXX_of_YYY;
} else
if (status bit ZZZ is set) {
  compute XXX_of_ZZZ;
} else
{ // default, no data available
  XXX = ZERO ?
  set UNINIT flag somewhere ?
}
}
--> do the same for the other states
--> do the same for the other states
--> maybe a part of this could be generated ?
--> we can use functions instead of macros
</pre>
</pre>

Revision as of 16:02, 19 February 2011

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 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
Retrieved from "http://wiki.paparazziuav.org/w/index.php?title=DevGuide/StateInterface&oldid=8744"