diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 29a86dda84..3e461ac70c 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -202,10 +202,9 @@ - - - + + +
diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index dfbcab0f90..154b0126fb 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -84,9 +84,8 @@ - - + +
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 75f60ddfdf..4deef180d9 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -163,6 +163,7 @@ +
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 099ab06ca6..8881af6298 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -186,8 +186,8 @@ - - + +
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 42fcb6b0d0..4a20c2b893 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -180,8 +180,8 @@ - - + +
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 7a858bf286..151464b016 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -190,8 +190,8 @@ - - + +
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index 1c3262b131..771f4082c8 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -199,7 +199,8 @@ More information on the Quadshot can be found at transition-robotics.com --> - + +
diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 8957d373cd..2f10e1c38b 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -186,7 +186,6 @@ -
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index f7e3935c5f..d57a70036b 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -224,7 +224,8 @@ - + +
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index ffbea72141..bb42616a4b 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -275,6 +275,7 @@ ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c +ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c # # INS choice diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index f57c246e8c..4785cbc075 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -119,6 +119,7 @@ nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c +nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c # # INS choice diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 1362a96a38..45c73a958b 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -7,7 +7,8 @@ - + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index c8cd29d6e1..432f655f1b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -24,32 +24,43 @@ * */ -#define GUIDANCE_V_C +#include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_v.h" - #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization.h" -// #include "booz_fms.h" FIXME #include "firmwares/rotorcraft/navigation.h" #include "state.h" #include "math/pprz_algebra_int.h" -#include "generated/airframe.h" - -/* warn if some gains are still negative */ +/* error if some gains are negative */ #if (GUIDANCE_V_HOVER_KP < 0) || \ (GUIDANCE_V_HOVER_KD < 0) || \ (GUIDANCE_V_HOVER_KI < 0) -#error "ALL control gains are now positive!!!" +#error "ALL control gains must be positive!!!" #endif + +/* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined, + * disable the adaptive throttle estimation by default. + * Otherwise enable adaptive estimation by default. + */ #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE -PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) +# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED +# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE +# endif +#else +# define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4 +# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED +# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE +# endif #endif +PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) +PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED) + uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; @@ -57,6 +68,7 @@ int32_t guidance_v_fb_cmd; int32_t guidance_v_delta_t; float guidance_v_nominal_throttle; +bool_t guidance_v_adapt_throttle_enabled; /** Direct throttle from radio control. @@ -91,7 +103,7 @@ int32_t guidance_v_z_sum_err; } -__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight); +void run_hover_loop(bool_t in_flight); void guidance_v_init(void) { @@ -104,9 +116,8 @@ void guidance_v_init(void) { guidance_v_z_sum_err = 0; -#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE; -#endif + guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED; gv_adapt_init(); } @@ -166,6 +177,10 @@ void guidance_v_run(bool_t in_flight) { if (in_flight) { gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); } + else { + /* reset estimate while not in_flight */ + gv_adapt_init(); + } switch (guidance_v_mode) { @@ -250,7 +265,7 @@ void guidance_v_run(bool_t in_flight) { #define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC)) -__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) { +void run_hover_loop(bool_t in_flight) { /* convert our reference to generic representation */ int64_t tmp = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC); @@ -271,13 +286,17 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig guidance_v_z_sum_err = 0; /* our nominal command : (g + zdd)*m */ -#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE - const int32_t inv_m = BFP_OF_REAL(9.81/(guidance_v_nominal_throttle*MAX_PPRZ), FF_CMD_FRAC); -#else - const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC); -#endif + int32_t inv_m; + if (guidance_v_adapt_throttle_enabled) { + inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC); + } + else { + /* use the fixed nominal throttle */ + inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC); + } + const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) - - (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC)); + (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC)); guidance_v_ff_cmd = g_m_zdd / inv_m; int32_t cphi,ctheta,cphitheta; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index a9c2f86403..4a94b4cfa8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -24,15 +24,13 @@ * */ -#ifndef GUIDANCE_V -#define GUIDANCE_V +#ifndef GUIDANCE_V_H +#define GUIDANCE_V_H #include "std.h" -#include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_v_ref.h" - -#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h" +#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h" #define GUIDANCE_V_MODE_KILL 0 #define GUIDANCE_V_MODE_RC_DIRECT 1 @@ -89,6 +87,10 @@ extern int32_t guidance_v_delta_t; */ extern float guidance_v_nominal_throttle; +/** Use adaptive throttle command estimation. + */ +extern bool_t guidance_v_adapt_throttle_enabled; + extern int32_t guidance_v_kp; ///< vertical control P-gain extern int32_t guidance_v_kd; ///< vertical control D-gain extern int32_t guidance_v_ki; ///< vertical control I-gain @@ -104,4 +106,4 @@ extern void guidance_v_run(bool_t in_flight); guidance_v_z_sum_err = 0; \ } -#endif /* GUIDANCE_V */ +#endif /* GUIDANCE_V_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c similarity index 58% rename from sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h rename to sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c index b22a8c188b..382a3cb0bb 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2009-2010 The Paparazzi Team + * Copyright (C) 2009-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -19,21 +19,51 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/guidance/guidance_v_adpt.h - * Adaptation bloc of the vertical guidance. +/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.c + * Adaptation block of the vertical guidance. * * This is a dimension one kalman filter estimating - * the ratio of vertical acceleration over thrust command ( ~ invert of the mass ) - * needed by the invert dynamic model to produce a nominal command + * the ratio of vertical acceleration over thrust command ( ~ inverse of the mass ) + * needed by the invert dynamic model to produce a nominal command. */ -#ifndef GUIDANCE_V_ADPT -#define GUIDANCE_V_ADPT - +#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h" #include "paparazzi.h" +#include "math/pprz_algebra_int.h" +#include "generated/airframe.h" + + +/** Initial hover throttle as factor of MAX_PPRZ. + * Should be a value between #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE and + * #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE. + * It is better to start with low thrust and let it rise as the adaptive filter + * finds the vehicle needs more thrust. + */ +#ifndef GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE +#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE 0.3 +#endif +PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE) + +/** Minimum hover throttle as factor of MAX_PPRZ. + * With the default of 0.2 the nominal hover throttle will + * never go lower than 20%. + */ +#ifndef GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE +#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE 0.2 +#endif +PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE) + +/** Maximum hover throttle as factor of MAX_PPRZ. + * With the default of 0.75 the nominal hover throttle will + * never go over 75% of max throttle. + */ +#ifndef GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE +#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE 0.75 +#endif +PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE) /** Adapt noise factor. - * Smaller values will make the filter to adapter faster. + * Smaller values will make the filter to adapt faster. * Bigger values (slower adaptation) make the filter more robust to external perturbations. * Factor should always be >0 */ @@ -41,14 +71,6 @@ #define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0 #endif -/** Initial estimation. - * The initial value can be adapted for faster converging time. - * It is usually recommended to start with a low value (overestimation of the mass), - * as it is helping for a smooth takeoff. - */ -#ifndef GUIDANCE_V_ADAPT_X0 -#define GUIDANCE_V_ADAPT_X0 0.003 -#endif /** Filter is not fed if accel values are more than +/- MAX_ACCEL. * MAX_ACCEL is a positive value in m/s^2 @@ -67,37 +89,12 @@ #define GUIDANCE_V_ADAPT_MIN_CMD 0.1 #endif -/** State of the estimator. - * fixed point representation with #GV_ADAPT_X_FRAC - * Q13.18 - */ -extern int32_t gv_adapt_X; -#define GV_ADAPT_X_FRAC 24 -/** Covariance. - * fixed point representation with #GV_ADAPT_P_FRAC - * Q13.18 - */ -extern int32_t gv_adapt_P; -#define GV_ADAPT_P_FRAC 18 - -/** Measurement */ -extern int32_t gv_adapt_Xmeas; - - -#ifdef GUIDANCE_V_C int32_t gv_adapt_X; int32_t gv_adapt_P; int32_t gv_adapt_Xmeas; - -/* Initial State and Covariance */ -#define GV_ADAPT_X0_F GUIDANCE_V_ADAPT_X0 -#define GV_ADAPT_X0 BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC) -#define GV_ADAPT_P0_F 0.1 -#define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC) - /* System noises */ #ifndef GV_ADAPT_SYS_NOISE_F #define GV_ADAPT_SYS_NOISE_F 0.00005 @@ -109,31 +106,15 @@ int32_t gv_adapt_Xmeas; #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC) #define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR) -/* Max accel */ -#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL) +/* Initial Covariance */ +#define GV_ADAPT_P0_F 0.1 +static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC); +static const int32_t gv_adapt_X0 = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / + (GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE * MAX_PPRZ); -/* Command bounds */ -#define GV_ADAPT_MAX_CMD ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ)) -#define GV_ADAPT_MIN_CMD ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ)) - -/* Output bounds. - * Don't let it climb over a value that would - * give less than zero throttle and don't let it down to zero. - * Worst cases: - * MIN_ACCEL / MAX_THROTTLE - * MAX_ACCEL / MIN_THROTTLE - * ex: - * 9.81*2^18/255 = 10084 - * 9.81*2^18/1 = 2571632 - */ -// TODO Check this properly -#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC)) -#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ) - - -static inline void gv_adapt_init(void) { - gv_adapt_X = GV_ADAPT_X0; - gv_adapt_P = GV_ADAPT_P0; +void gv_adapt_init(void) { + gv_adapt_X = gv_adapt_X0; + gv_adapt_P = gv_adapt_P0; } #define K_FRAC 12 @@ -143,12 +124,16 @@ static inline void gv_adapt_init(void) { * @param thrust_applied controller input [0 : MAX_PPRZ] * @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC */ -static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { +void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { + + static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ; + static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ; + static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL); /* Update only if accel and commands are in a valid range */ /* This also ensures we don't divide by zero */ - if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD - || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) { + if (thrust_applied < gv_adapt_min_cmd || thrust_applied > gv_adapt_max_cmd + || zdd_meas < -gv_adapt_max_accel || zdd_meas > gv_adapt_max_accel) { return; } @@ -178,18 +163,21 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_ /* Update Covariance Pnew = P - K * P */ gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC); /* Don't let covariance climb over initial value */ - if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0; + if (gv_adapt_P > gv_adapt_P0) { + gv_adapt_P = gv_adapt_P0; + } /* Update State */ gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual))>>K_FRAC); - /* Again don't let it climb over a value that would - * give less than zero throttle and don't let it down to zero. + /* Output bounds. + * Don't let it climb over a value that would + * give less than #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE % throttle + * or more than #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE % throttle. */ - Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT); + static const int32_t max_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / + (GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE * MAX_PPRZ); + static const int32_t min_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / + (GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ); + Bound(gv_adapt_X, min_out, max_out); } - - -#endif /* GUIDANCE_V_C */ - -#endif /* GUIDANCE_V_ADPT */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h new file mode 100644 index 0000000000..8be2e35a22 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2009-2013 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.h + * Adaptation block of the vertical guidance. + * + * This is a dimension one kalman filter estimating + * the ratio of vertical acceleration over thrust command ( ~ inverse of the mass ) + * needed by the invert dynamic model to produce a nominal command. + */ + +#ifndef GUIDANCE_V_ADAPT_H +#define GUIDANCE_V_ADAPT_H + +#include "std.h" + +/** State of the estimator. + * fixed point representation with #GV_ADAPT_X_FRAC + * Q13.18 + */ +extern int32_t gv_adapt_X; +#define GV_ADAPT_X_FRAC 24 + +/** Covariance. + * fixed point representation with #GV_ADAPT_P_FRAC + * Q13.18 + */ +extern int32_t gv_adapt_P; +#define GV_ADAPT_P_FRAC 18 + +/** Measurement */ +extern int32_t gv_adapt_Xmeas; + + +extern void gv_adapt_init(void); +extern void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref); + + +#endif /* GUIDANCE_V_ADAPT_H */