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 ltp_def;
  bool_t ltp_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;

  /* wind and airspeed*/
  struct Int32Vect3 ned_airspeed;
  struct Int32Vect3 ned_windspeed;
};

struct StateFloat {
  /* 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 hspeed_norm;
  float hspeed_dir;

  struct FloatVect2 windspeed; /* m/s ; x = north, y = east */
  float airspeed; /* m/s */

  struct FloatEulers ned_to_body_euler;
  struct FloatRates  body_rate;
}

So we can either have two state (3 if we want double as well?) structures, or we put everything into one...

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

Quaternion and rotation matrix in float as well?

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