DevGuide/StateInterface

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

state.h

/* abstract state interface */
struct State {
  /* Earth Centered Earth Fixed in centimeters */
  struct EcefCoor_i ecef_pos;

  /* lon, lat in radians*1e7  */
  /* alt in centimeters above MSL  */
  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;
  struct NedCoor_i ned_speed;
  struct NedCoor_i ned_accel;

  /* vehicle attitude */
  struct Int32Quat   ned_to_body_quat;
  struct Int32Eulers ned_to_body_euler;
  struct Int32RMat   ned_to_body_rmat;
  struct Int32Rates  body_rate;

  /* horizontal windspeed x = north, y = east */
  struct Int32Vect2 h_windspeed;

  struct int32_t airspeed_norm;

  /***** float representations ****/

  /* 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 */
  struct FloatVect3 ned_accel;

  /* speed in North East Down local tangent plane */
  struct FloatVect3 ned_speed;
  /* horizontal ground speed in norm and dir (m/s, rad (CW/North)) */
  float h_speed_norm;
  float h_speed_dir;

  struct FloatVect2 h_windspeed; /* m/s ; x = north, y = east */
  float airspeed_norm; /* m/s */

  struct FloatQuat   ned_to_body_quat;
  struct FloatEulers ned_to_body_euler;
  struct FloatRMat   ned_to_body_rmat;
  struct FloatRates  body_rate;
}

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).

How to set/get the state ?

State accessor functions we need:

Integer set versions (inline 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)
• 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)
• StateSetAirspeedNorm(int32_t airspeed_norm)

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 XXX 1<<0
#define YYY 1<<1
#define ZZZ 1<<2
uint XYZ_status;

// set the new value and reset the status to tell that other format are not up to date anymore
#define STATE_SET_XXX(_v) {
 state.XXX = _v;
 pos_status = XXX;
}
--> do the same for YYY and ZZZ, and even for XXX_YYY, XXX_ZZZ, ... if needed

// get the value and transformation if needed
#define STATE_GET_XXX(_r) {
 if (status bit XXX is not set) {
  transform_XXX();
  set bit XXX in status;
 }
 _r = XXX;
}
--> 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

--> maybe a part of this could be generated ?
--> we can use functions instead of macros