Difference between revisions of "DevGuide/StateInterface"

From PaparazziUAV
Jump to navigation Jump to search
 
(2 intermediate revisions by 2 users not shown)
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 general state interface holds all the most important vehicle states like position, velocity, attitude, etc.
It handles coordinate system and fixed-/floating-point conversion on the fly and only when needed.


See the [https://github.com/paparazzi/paparazzi/issues/101 github issue] and [https://github.com/paparazzi/paparazzi/tree/state_interface state_interface branch] for the current status.
'''See the [http://docs.paparazziuav.org/latest/group__state__interface.html generated documentation] for more details.'''


=== state.h ===
The state interface was merged into the master branch with [https://github.com/paparazzi/paparazzi/commit/dbdbbaf6c265c2abeaafcb588e39d2fa6a33536f commit dbdbbaf].
<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 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);
</pre>
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,...
<pre>
#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
</pre>


[[Category:Software]] [[Category:Developer_Documentation]]
[[Category:Software]] [[Category:Developer_Documentation]]

Latest revision as of 07:16, 3 August 2014

The general state interface holds all the most important vehicle states like position, velocity, attitude, etc. It handles coordinate system and fixed-/floating-point conversion on the fly and only when needed.

See the generated documentation for more details.

The state interface was merged into the master branch with commit dbdbbaf.

Retrieved from "http://wiki.paparazziuav.org/w/index.php?title=DevGuide/StateInterface&oldid=18880"