diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index a1d843231e..9dce60427f 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -239,7 +239,9 @@ ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c ap.CFLAGS += -DUSE_NAVIGATION 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 # # INS choice diff --git a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile index ece85d0f60..53d7b44202 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile @@ -5,7 +5,9 @@ stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization.c stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c +stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c +stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c stm_passthrough.CFLAGS += -DUSE_NAVIGATION stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 0272f455b0..f4b19af9fa 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -113,7 +113,9 @@ nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c nps.CFLAGS += -DUSE_NAVIGATION 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 # # INS choice diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 8e72a4899f..dc42506ed8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -24,8 +24,6 @@ * */ -#define GUIDANCE_H_C - #include "firmwares/rotorcraft/guidance/guidance_h.h" #include "firmwares/rotorcraft/stabilization.h" @@ -84,7 +82,7 @@ static inline void guidance_h_hover_enter(void); static inline void guidance_h_nav_enter(void); #define GuidanceHSetRef(_pos, _speed, _accel) { \ - b2_gh_set_ref(_pos, _speed, _accel); \ + gh_set_ref(_pos, _speed, _accel); \ VECT2_COPY(guidance_h_pos_ref, _pos); \ VECT2_COPY(guidance_h_speed_ref, _speed); \ VECT2_COPY(guidance_h_accel_ref, _accel); \ @@ -242,10 +240,10 @@ void guidance_h_run(bool_t in_flight) { static inline void guidance_h_update_reference(bool_t use_ref) { /* convert our reference to generic representation */ if (use_ref) { - b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp); - INT32_VECT2_RSHIFT(guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC)); - INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC)); - INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC)); + gh_update_ref_from_pos_sp(guidance_h_pos_sp); + INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); } else { VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp); INT_VECT2_ZERO(guidance_h_speed_ref); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c new file mode 100644 index 0000000000..1542f639fc --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -0,0 +1,244 @@ +/* + * Copyright (C) 2008-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_h_ref.c + * Reference generation for horizontal guidance. + * + */ + +#include "firmwares/rotorcraft/guidance/guidance_h_ref.h" +#include "generated/airframe.h" + +/** Reference model acceleration. + * in meters/sec2 (output) + * fixed point representation: Q23.8 + * accuracy 0.0039, range 8388km/s2 + */ +struct Int32Vect2 gh_accel_ref; + +/** Reference model speed. + * in meters/sec + * with fixedpoint representation: Q14.17 + * accuracy 0.0000076 , range 16384m/s + */ +struct Int32Vect2 gh_speed_ref; + +/* Reference model position. + * in meters + * with fixedpoint representation: Q37.26 + */ +struct Int64Vect2 gh_pos_ref; + +/** Accel saturation. + * tanf(RadOfDeg(30.))*9.81 = 5.66 + */ +#ifndef GUIDANCE_H_REF_MAX_ACCEL +#define GUIDANCE_H_REF_MAX_ACCEL 5.66 +#endif +#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC) + +/** Speed saturation */ +#ifndef GUIDANCE_H_REF_MAX_SPEED +#define GUIDANCE_H_REF_MAX_SPEED 5. +#endif +/** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ +#define GH_MAX_SPEED_REF_FRAC 7 +#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC) + +/** second order model natural frequency */ +#ifndef GUIDANCE_H_REF_OMEGA +#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.) +#endif +/** second order model damping */ +#ifndef GUIDANCE_H_REF_ZETA +#define GUIDANCE_H_REF_ZETA 0.85 +#endif +#define GH_ZETA_OMEGA_FRAC 10 +#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC) +#define GH_OMEGA_2_FRAC 7 +#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC) + +/** first order time constant */ +#define GH_REF_THAU_F 0.5 +#define GH_REF_INV_THAU_FRAC 16 +#define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC) + +static struct Int32Vect2 gh_max_speed_ref; +static struct Int32Vect2 gh_max_accel_ref; + +static int32_t route_ref; +static int32_t s_route_ref; +static int32_t c_route_ref; + +void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { + struct Int64Vect2 new_pos; + new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); + new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); + gh_pos_ref = new_pos; + INT32_VECT2_RSHIFT(gh_speed_ref, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); + INT32_VECT2_RSHIFT(gh_accel_ref, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); +} + +void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { + + VECT2_ADD(gh_pos_ref, gh_speed_ref); + VECT2_ADD(gh_speed_ref, gh_accel_ref); + + // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) + struct Int32Vect2 speed; + INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); + VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA); + INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); + // compute pos error in pos_sp resolution + struct Int32Vect2 pos_err; + INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); + VECT2_DIFF(pos_err, pos_err, pos_sp); + // convert to accel resolution + INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC)); + // compute the "pos part" of accel + struct Int32Vect2 pos; + VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2)); + INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC); + // sum accel + VECT2_SUM(gh_accel_ref, speed, pos); + + /* Compute route reference before saturation */ + // use metric precision or values are too large + INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x); + route_ref = abs(route_ref); + /* Compute North and East route components */ + PPRZ_ITRIG_SIN(s_route_ref, route_ref); + PPRZ_ITRIG_COS(c_route_ref, route_ref); + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + + /* Saturate accelerations */ + if (gh_accel_ref.x <= -gh_max_accel_ref.x) { + gh_accel_ref.x = -gh_max_accel_ref.x; + } + else if (gh_accel_ref.x >= gh_max_accel_ref.x) { + gh_accel_ref.x = gh_max_accel_ref.x; + } + if (gh_accel_ref.y <= -gh_max_accel_ref.y) { + gh_accel_ref.y = -gh_max_accel_ref.y; + } + else if (gh_accel_ref.y >= gh_max_accel_ref.y) { + gh_accel_ref.y = gh_max_accel_ref.y; + } + + /* Saturate speed and adjust acceleration accordingly */ + if (gh_speed_ref.x <= -gh_max_speed_ref.x) { + gh_speed_ref.x = -gh_max_speed_ref.x; + if (gh_accel_ref.x < 0) + gh_accel_ref.x = 0; + } + else if (gh_speed_ref.x >= gh_max_speed_ref.x) { + gh_speed_ref.x = gh_max_speed_ref.x; + if (gh_accel_ref.x > 0) + gh_accel_ref.x = 0; + } + if (gh_speed_ref.y <= -gh_max_speed_ref.y) { + gh_speed_ref.y = -gh_max_speed_ref.y; + if (gh_accel_ref.y < 0) + gh_accel_ref.y = 0; + } + else if (gh_speed_ref.y >= gh_max_speed_ref.y) { + gh_speed_ref.y = gh_max_speed_ref.y; + if (gh_accel_ref.y > 0) + gh_accel_ref.y = 0; + } +} + +void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { +/* WARNING: SPEED SATURATION UNTESTED */ + VECT2_ADD(gh_pos_ref, gh_speed_ref); + VECT2_ADD(gh_speed_ref, gh_accel_ref); + + // compute speed error + struct Int32Vect2 speed_err; + INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); + VECT2_DIFF(speed_err, gh_speed_ref, speed_err); + // convert to accel resolution + INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); + // compute accel from speed_sp + VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU); + INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC); + + /* Compute route reference before saturation */ + // use metric precision or values are too large + INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x); + route_ref = abs(route_ref); + /* Compute North and East route components */ + PPRZ_ITRIG_SIN(s_route_ref, route_ref); + PPRZ_ITRIG_COS(c_route_ref, route_ref); + + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + + /* Saturate accelerations */ + if (gh_accel_ref.x <= -gh_max_accel_ref.x) { + gh_accel_ref.x = -gh_max_accel_ref.x; + } + else if (gh_accel_ref.x >= gh_max_accel_ref.x) { + gh_accel_ref.x = gh_max_accel_ref.x; + } + if (gh_accel_ref.y <= -gh_max_accel_ref.y) { + gh_accel_ref.y = -gh_max_accel_ref.y; + } + else if (gh_accel_ref.y >= gh_max_accel_ref.y) { + gh_accel_ref.y = gh_max_accel_ref.y; + } + + /* Saturate speed and adjust acceleration accordingly */ + if (gh_speed_ref.x <= -gh_max_speed_ref.x) { + gh_speed_ref.x = -gh_max_speed_ref.x; + if (gh_accel_ref.x < 0) + gh_accel_ref.x = 0; + } + else if (gh_speed_ref.x >= gh_max_speed_ref.x) { + gh_speed_ref.x = gh_max_speed_ref.x; + if (gh_accel_ref.x > 0) + gh_accel_ref.x = 0; + } + if (gh_speed_ref.y <= -gh_max_speed_ref.y) { + gh_speed_ref.y = -gh_max_speed_ref.y; + if (gh_accel_ref.y < 0) + gh_accel_ref.y = 0; + } + else if (gh_speed_ref.y >= gh_max_speed_ref.y) { + gh_speed_ref.y = gh_max_speed_ref.y; + if (gh_accel_ref.y > 0) + gh_accel_ref.y = 0; + } +} + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index b70c12d7cf..ab285baa11 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -27,244 +27,40 @@ #ifndef GUIDANCE_H_REF_H #define GUIDANCE_H_REF_H -#include "generated/airframe.h" #include "inttypes.h" #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" -/* update frequency */ -#define B2_GH_FREQ_FRAC 9 -#define B2_GH_FREQ (1<= b2_gh_max_accel_ref.x) { - b2_gh_accel_ref.x = b2_gh_max_accel_ref.x; - } - if (b2_gh_accel_ref.y <= -b2_gh_max_accel_ref.y) { - b2_gh_accel_ref.y = -b2_gh_max_accel_ref.y; - } - else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) { - b2_gh_accel_ref.y = b2_gh_max_accel_ref.y; - } - - /* Saturate speed and adjust acceleration accordingly */ - if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) { - b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x; - if (b2_gh_accel_ref.x < 0) - b2_gh_accel_ref.x = 0; - } - else if (b2_gh_speed_ref.x >= b2_gh_max_speed_ref.x) { - b2_gh_speed_ref.x = b2_gh_max_speed_ref.x; - if (b2_gh_accel_ref.x > 0) - b2_gh_accel_ref.x = 0; - } - if (b2_gh_speed_ref.y <= -b2_gh_max_speed_ref.y) { - b2_gh_speed_ref.y = -b2_gh_max_speed_ref.y; - if (b2_gh_accel_ref.y < 0) - b2_gh_accel_ref.y = 0; - } - else if (b2_gh_speed_ref.y >= b2_gh_max_speed_ref.y) { - b2_gh_speed_ref.y = b2_gh_max_speed_ref.y; - if (b2_gh_accel_ref.y > 0) - b2_gh_accel_ref.y = 0; - } -} - -static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { -/* WARNING: SPEED SATURATION UNTESTED */ - VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref); - VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref); - - // compute speed error - struct Int32Vect2 speed_err; - INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC)); - VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err); - // convert to accel resolution - INT32_VECT2_RSHIFT(speed_err, speed_err, (B2_GH_SPEED_REF_FRAC - B2_GH_ACCEL_REF_FRAC)); - // compute accel from speed_sp - VECT2_SMUL(b2_gh_accel_ref, speed_err, -B2_GH_REF_INV_THAU); - INT32_VECT2_RSHIFT(b2_gh_accel_ref, b2_gh_accel_ref, B2_GH_REF_INV_THAU_FRAC); - - /* Compute route reference before saturation */ - // use metric precision or values are too large - INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x); - /* Compute North and East route components */ - PPRZ_ITRIG_SIN(s_route_ref, route_ref); - PPRZ_ITRIG_COS(c_route_ref, route_ref); - c_route_ref = abs(c_route_ref); - s_route_ref = abs(s_route_ref); - - /* Compute maximum acceleration*/ - b2_gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); - b2_gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - b2_gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); - b2_gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); - /* restore b2_gh_speed_ref range (Q14.17) */ - INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC)); - - /* Saturate accelerations */ - if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) { - b2_gh_accel_ref.x = -b2_gh_max_accel_ref.x; - } - else if (b2_gh_accel_ref.x >= b2_gh_max_accel_ref.x) { - b2_gh_accel_ref.x = b2_gh_max_accel_ref.x; - } - if (b2_gh_accel_ref.y <= -b2_gh_max_accel_ref.y) { - b2_gh_accel_ref.y = -b2_gh_max_accel_ref.y; - } - else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) { - b2_gh_accel_ref.y = b2_gh_max_accel_ref.y; - } - - /* Saturate speed and adjust acceleration accordingly */ - if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) { - b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x; - if (b2_gh_accel_ref.x < 0) - b2_gh_accel_ref.x = 0; - } - else if (b2_gh_speed_ref.x >= b2_gh_max_speed_ref.x) { - b2_gh_speed_ref.x = b2_gh_max_speed_ref.x; - if (b2_gh_accel_ref.x > 0) - b2_gh_accel_ref.x = 0; - } - if (b2_gh_speed_ref.y <= -b2_gh_max_speed_ref.y) { - b2_gh_speed_ref.y = -b2_gh_max_speed_ref.y; - if (b2_gh_accel_ref.y < 0) - b2_gh_accel_ref.y = 0; - } - else if (b2_gh_speed_ref.y >= b2_gh_max_speed_ref.y) { - b2_gh_speed_ref.y = b2_gh_max_speed_ref.y; - if (b2_gh_accel_ref.y > 0) - b2_gh_accel_ref.y = 0; - } -} - -#endif /* GUIDANCE_H_C */ +extern void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel); +extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp); +extern void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp); #endif /* GUIDANCE_H_REF_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 901731e060..2287d88252 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -25,7 +25,6 @@ */ #define GUIDANCE_V_C -#define GUIDANCE_V_USE_REF 1 #include "firmwares/rotorcraft/guidance/guidance_v.h" diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c new file mode 100644 index 0000000000..281827bb0b --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2013 Gautier Hattenberger + * + * 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_ref.c + * Reference generation for vertical guidance. + * + */ + +#include "firmwares/rotorcraft/guidance/guidance_v_ref.h" +#include "generated/airframe.h" + +/** reference model vertical accel in meters/s^2 (output) + * fixed point representation with #GV_ZDD_REF_FRAC + * Q23.8 : accuracy 0.0039 , range 8388km/s^2 + */ +int32_t gv_zdd_ref; + +/** reference model vertical speed in meters/sec (output) + * fixed point representation with #GV_ZD_REF_FRAC + * Q14.17 : accuracy 0.0000076 , range 16384m/s2 + */ +int32_t gv_zd_ref; + +/** reference model altitude in meters (output) + * fixed point representation with #GV_Z_REF_FRAC + * Q37.26 : + */ +int64_t gv_z_ref; + + +/* Saturations definition */ +#ifndef GUIDANCE_V_REF_MIN_ZDD +#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81) +#endif +#define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC) + +#ifndef GUIDANCE_V_REF_MAX_ZDD +#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81) +#endif +#define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC) + +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) +#endif +#define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC) + +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) +#endif +#define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC) + +/* second order model natural frequency and damping */ +#ifndef GUIDANCE_V_REF_OMEGA +#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.) +#endif +#ifndef GUIDANCE_V_REF_ZETA +#define GUIDANCE_V_REF_ZETA 0.85 +#endif +#define GV_ZETA_OMEGA_FRAC 10 +#define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC) +#define GV_OMEGA_2_FRAC 7 +#define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC) + +/* first order time constant */ +#define GV_REF_THAU_F 0.25 +#define GV_REF_INV_THAU_FRAC 16 +#define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC) + +void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) { + int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC); + gv_z_ref = new_z; + gv_zd_ref = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); + gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); +} + +void gv_update_ref_from_z_sp(int32_t z_sp) { + + gv_z_ref += gv_zd_ref; + gv_zd_ref += gv_zdd_ref; + + // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z) + int32_t zd_zdd_res = gv_zd_ref>>(GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC); + int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC); + // compute z error in z_sp resolution + int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC)); + // convert to accel resolution + int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC); + int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC; + gv_zdd_ref = zdd_speed + zdd_pos; + + /* Saturate accelerations */ + Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD); + + /* Saturate speed and adjust acceleration accordingly */ + if (gv_zd_ref <= GV_MIN_ZD) { + gv_zd_ref = GV_MIN_ZD; + if (gv_zdd_ref < 0) + gv_zdd_ref = 0; + } + else if (gv_zd_ref >= GV_MAX_ZD) { + gv_zd_ref = GV_MAX_ZD; + if (gv_zdd_ref > 0) + gv_zdd_ref = 0; + } +} + + +void gv_update_ref_from_zd_sp(int32_t zd_sp) { + + gv_z_ref += gv_zd_ref; + gv_zd_ref += gv_zdd_ref; + + int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC)); + int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC); + gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC; + + /* Saturate accelerations */ + Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD); + + /* Saturate speed and adjust acceleration accordingly */ + if (gv_zd_ref <= GV_MIN_ZD) { + gv_zd_ref = GV_MIN_ZD; + if (gv_zdd_ref < 0) + gv_zdd_ref = 0; + } + else if (gv_zd_ref >= GV_MAX_ZD) { + gv_zd_ref = GV_MAX_ZD; + if (gv_zdd_ref > 0) + gv_zdd_ref = 0; + } +} + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index 6a19764159..f462a07a2b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -27,12 +28,12 @@ #ifndef GUIDANCE_V_REF_H #define GUIDANCE_V_REF_H -#include "generated/airframe.h" #include "inttypes.h" #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" -/* update frequency */ +/** Update frequency + */ #define GV_FREQ_FRAC 9 #define GV_FREQ (1<>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); - gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); -} - -__attribute__ ((always_inline)) static inline void gv_update_ref_from_z_sp(int32_t z_sp) { - - gv_z_ref += gv_zd_ref; - gv_zd_ref += gv_zdd_ref; - - // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z) - int32_t zd_zdd_res = gv_zd_ref>>(GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC); - int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC); - // compute z error in z_sp resolution - int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC)); - // convert to accel resolution - int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC); - int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC; - gv_zdd_ref = zdd_speed + zdd_pos; - - /* Saturate accelerations */ - Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD); - - /* Saturate speed and adjust acceleration accordingly */ - if (gv_zd_ref <= GV_MIN_ZD) { - gv_zd_ref = GV_MIN_ZD; - if (gv_zdd_ref < 0) - gv_zdd_ref = 0; - } - else if (gv_zd_ref >= GV_MAX_ZD) { - gv_zd_ref = GV_MAX_ZD; - if (gv_zdd_ref > 0) - gv_zdd_ref = 0; - } -} - - -__attribute__ ((always_inline)) static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) { - - gv_z_ref += gv_zd_ref; - gv_zd_ref += gv_zdd_ref; - - int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC)); - int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC); - gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC; - - /* Saturate accelerations */ - Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD); - - /* Saturate speed and adjust acceleration accordingly */ - if (gv_zd_ref <= GV_MIN_ZD) { - gv_zd_ref = GV_MIN_ZD; - if (gv_zdd_ref < 0) - gv_zdd_ref = 0; - } - else if (gv_zd_ref >= GV_MAX_ZD) { - gv_zd_ref = GV_MAX_ZD; - if (gv_zdd_ref > 0) - gv_zdd_ref = 0; - } -} - -#endif /* GUIDANCE_V_C */ +extern void gv_set_ref(int32_t alt, int32_t speed, int32_t accel); +extern void gv_update_ref_from_z_sp(int32_t z_sp); +extern void gv_update_ref_from_zd_sp(int32_t zd_sp); #endif /* GUIDANCE_V_REF_H */