diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c index 9741085340..531c6eb3ba 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_h.c +++ b/sw/airborne/booz/guidance/booz2_guidance_h.c @@ -21,6 +21,8 @@ * Boston, MA 02111-1307, USA. */ +#define B2_GUIDANCE_H_C +//#define B2_GUIDANCE_H_USE_REF #include "booz2_guidance_h.h" #include "booz_ahrs.h" @@ -34,7 +36,10 @@ uint8_t booz2_guidance_h_mode; struct Int32Vect2 booz2_guidance_h_pos_sp; -int32_t booz2_guidance_h_psi_sp; +int32_t booz2_guidance_h_psi_sp; +struct Int32Vect2 booz2_guidance_h_pos_ref; +struct Int32Vect2 booz2_guidance_h_speed_ref; +struct Int32Vect2 booz2_guidance_h_accel_ref; struct Int32Vect2 booz2_guidance_h_pos_err; struct Int32Vect2 booz2_guidance_h_speed_err; @@ -65,6 +70,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight); static inline void booz2_guidance_h_hover_enter(void); static inline void booz2_guidance_h_nav_enter(void); +#define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \ + b2_gh_set_ref(_pos, _speed, _accel); \ + VECT2_COPY(booz2_guidance_h_pos_ref, _pos); \ + VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \ + VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \ + } + void booz2_guidance_h_init(void) { @@ -179,6 +191,9 @@ void booz2_guidance_h_run(bool_t in_flight) { } else { INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot); +#ifdef B2_GUIDANCE_H_USE_REF + b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp); +#endif #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); #endif @@ -263,13 +278,25 @@ static inline void booz2_guidance_h_hover_run(void) { static inline void booz2_guidance_h_nav_run(bool_t in_flight) { + /* convert our reference to generic representation */ +#ifdef B2_GUIDANCE_H_USE_REF + INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC)); + INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC)); + INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC)); +#else + VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp); + INT_VECT2_ZERO(booz2_guidance_v_speed_ref); + INT_VECT2_ZERO(booz2_guidance_v_accel_ref); +#endif + /* compute position error */ - VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, booz2_guidance_h_pos_sp); + VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, booz2_guidance_h_pos_ref); /* saturate it */ VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); /* compute speed error */ - VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed); + //VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed); + VECT2_DIFF(booz2_guidance_h_speed_err, booz_ins_ltp_speed, booz2_guidance_h_speed_ref); /* saturate it */ VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); @@ -305,13 +332,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) { booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) + booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - 10)) + booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x + - booz2_guidance_h_again * booz_ins_ltp_accel.x; + booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain */ booz2_guidance_h_command_earth.y = booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) + booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) + booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - 10)) + booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y + - booz2_guidance_h_again * booz_ins_ltp_accel.y; + booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain */ VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK); INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth, booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for trigo operation @@ -327,11 +354,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) { booz2_guidance_h_command_body.theta = - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16)); + // Add RC setpoint booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi; booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta; booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi; ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi); + // Set attitude setpoint EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body); } @@ -352,6 +381,10 @@ static inline void booz2_guidance_h_hover_enter(void) { static inline void booz2_guidance_h_nav_enter(void) { INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot); + struct Int32Vect2 pos,speed,zero; + VECT2_COPY(pos, booz_ins_ltp_pos); + VECT2_COPY(speed, booz_ins_ltp_speed); + Booz2GuidanceHSetRef(pos, speed, zero); struct Int32Eulers tmp_sp; BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp ); diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.h b/sw/airborne/booz/guidance/booz2_guidance_h.h index d12968b444..5ca8adcaf9 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_h.h +++ b/sw/airborne/booz/guidance/booz2_guidance_h.h @@ -27,6 +27,8 @@ #include "math/pprz_algebra_int.h" +#include "booz2_guidance_h_ref.h" + #define BOOZ2_GUIDANCE_H_MODE_KILL 0 #define BOOZ2_GUIDANCE_H_MODE_RATE 1 #define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2 @@ -40,6 +42,9 @@ extern uint8_t booz2_guidance_h_mode; /* Q_int32_xx_8 */ extern struct Int32Vect2 booz2_guidance_h_pos_sp; extern int32_t booz2_guidance_h_psi_sp; +extern struct Int32Vect2 booz2_guidance_h_pos_ref; +extern struct Int32Vect2 booz2_guidance_h_speed_ref; +extern struct Int32Vect2 booz2_guidance_h_accel_ref; extern struct Int32Vect2 booz2_guidance_h_pos_err; extern struct Int32Vect2 booz2_guidance_h_speed_err; diff --git a/sw/airborne/booz/guidance/booz2_guidance_h_ref.h b/sw/airborne/booz/guidance/booz2_guidance_h_ref.h new file mode 100644 index 0000000000..b6cf06be8e --- /dev/null +++ b/sw/airborne/booz/guidance/booz2_guidance_h_ref.h @@ -0,0 +1,192 @@ +/* + * $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $ + * + * Copyright (C) 2008-2009 ENAC + * + * 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. + */ + +#ifndef BOOZ2_GUIDANCE_H_REF_H +#define BOOZ2_GUIDANCE_H_REF_H + +#include "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_SPEED) { + b2_gh_speed_ref.x = B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.x > 0) + b2_gh_accel_ref.x = 0; + } + if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) { + b2_gh_speed_ref.y = -B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.y < 0) + b2_gh_accel_ref.y = 0; + } + else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) { + b2_gh_speed_ref.y = B2_GH_MAX_SPEED; + 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) { + + 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); + + /* Saturate accelerations */ + VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL); + + /* Saturate speed and adjust acceleration accordingly */ + if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) { + b2_gh_speed_ref.x = -B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.x < 0) + b2_gh_accel_ref.x = 0; + } + else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) { + b2_gh_speed_ref.x = B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.x > 0) + b2_gh_accel_ref.x = 0; + } + if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) { + b2_gh_speed_ref.y = -B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.y < 0) + b2_gh_accel_ref.y = 0; + } + else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) { + b2_gh_speed_ref.y = B2_GH_MAX_SPEED; + if (b2_gh_accel_ref.y > 0) + b2_gh_accel_ref.y = 0; + } +} + +#endif /* B2_GUIDANCE_H_C */ + +#endif /* BOOZ2_GUIDANCE_H_REF_H */ +