diff --git a/conf/airframes/booz2_a5.xml b/conf/airframes/booz2_a5.xml index 7c3b952a68..dab7192289 100644 --- a/conf/airframes/booz2_a5.xml +++ b/conf/airframes/booz2_a5.xml @@ -19,6 +19,9 @@ test_telemetry.CFLAGS += -I$(GLIB_INC) test_telemetry.LDFLAGS += -L$(GLIB_LIB) -lglib-2.0 +lpc_test_telemetry.ARCHDIR= + + ap.ARCHDIR = $(ARCHI) diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile index de19238962..423308b76a 100644 --- a/conf/autopilot/booz2_simulator_nps.makefile +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -93,7 +93,11 @@ sim.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c sim.srcs += $(SRC_BOOZ)/booz_stabilization.c sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c -sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude.c +#sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_H=\"stabilization/booz_stabilization_attitude_euler.h\" +#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler.c +sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c + + sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c new file mode 100644 index 0000000000..ad9d0742df --- /dev/null +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c @@ -0,0 +1,194 @@ +/* + * $Id: booz_stabilization_attitude_euler.c 3787 2009-07-24 15:33:54Z poine $ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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. + */ + +#include "booz_stabilization.h" +#include "booz_ahrs.h" +#include "booz_radio_control.h" + +#include "airframe.h" + +struct Int32Eulers booz_stabilization_att_sp; + +struct Int32Eulers booz_stabilization_att_ref; +struct Int32Vect3 booz_stabilization_rate_ref; +struct Int32Vect3 booz_stabilization_accel_ref; + +struct Int32Vect3 booz_stabilization_pgain; +struct Int32Vect3 booz_stabilization_dgain; +struct Int32Vect3 booz_stabilization_ddgain; +struct Int32Vect3 booz_stabilization_igain; +struct Int32Eulers booz_stabilization_att_sum_err; + +int32_t booz_stabilization_att_err_cmd[COMMANDS_NB]; + +static inline void booz_stabilization_update_ref(void); + + +void booz_stabilization_attitude_init(void) { + + INT_EULERS_ZERO(booz_stabilization_att_sp); + + INT_EULERS_ZERO(booz_stabilization_att_ref); + INT_VECT3_ZERO(booz_stabilization_rate_ref); + INT_VECT3_ZERO(booz_stabilization_accel_ref); + + VECT3_ASSIGN(booz_stabilization_pgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN); + + VECT3_ASSIGN(booz_stabilization_dgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN); + + VECT3_ASSIGN(booz_stabilization_ddgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN); + + VECT3_ASSIGN(booz_stabilization_igain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN); + + INT_EULERS_ZERO( booz_stabilization_att_sum_err ); + +} + + +void booz_stabilization_attitude_read_rc(bool_t in_flight) { + + BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stabilization_att_sp, in_flight); + +} + + +void booz_stabilization_attitude_enter(void) { + + BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stabilization_att_sp ); + INT_EULERS_ZERO( booz_stabilization_att_sum_err ); + +} + + +#define MAX_SUM_ERR 4000000 + +void booz_stabilization_attitude_run(bool_t in_flight) { + + booz_stabilization_update_ref(); + + /* compute attitude error */ + const struct Int32Eulers att_ref_scaled = { + booz_stabilization_att_ref.phi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC), + booz_stabilization_att_ref.theta >> (ANGLE_REF_RES - INT32_ANGLE_FRAC), + booz_stabilization_att_ref.psi >> (ANGLE_REF_RES - INT32_ANGLE_FRAC) }; + struct Int32Eulers att_err; + EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled); + INT32_ANGLE_NORMALIZE(att_err.psi); + + if (in_flight) { + /* update integrator */ + EULERS_ADD(booz_stabilization_att_sum_err, att_err); + EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); + } + else { + INT_EULERS_ZERO(booz_stabilization_att_sum_err); + } + + /* compute rate error */ + const struct Int32Rates rate_ref_scaled = { + booz_stabilization_rate_ref.x >> (RATE_REF_RES - INT32_RATE_FRAC), + booz_stabilization_rate_ref.y >> (RATE_REF_RES - INT32_RATE_FRAC), + booz_stabilization_rate_ref.z >> (RATE_REF_RES - INT32_RATE_FRAC) }; + struct Int32Rates rate_err; + RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled); + + /* compute PID loop */ + + booz_stabilization_att_err_cmd[COMMAND_ROLL] = + booz_stabilization_pgain.x * att_err.phi + + booz_stabilization_dgain.x * rate_err.p + + ((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi) >> 10); + booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_att_err_cmd[COMMAND_ROLL] + + ((booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x) >> 5); + booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_cmd[COMMAND_ROLL] >> 16; + + booz_stabilization_att_err_cmd[COMMAND_PITCH] = + booz_stabilization_pgain.y * att_err.theta + + booz_stabilization_dgain.y * rate_err.q + + ((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta) >> 10); + booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_att_err_cmd[COMMAND_PITCH] + + ((booz_stabilization_ddgain.y * booz_stabilization_accel_ref.y) >> 5); + booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_cmd[COMMAND_PITCH] >> 16; + + booz_stabilization_att_err_cmd[COMMAND_YAW] = + booz_stabilization_pgain.z * att_err.psi + + booz_stabilization_dgain.z * rate_err.r + + ((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi) >> 10); + booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_att_err_cmd[COMMAND_YAW] + + ((booz_stabilization_ddgain.z * booz_stabilization_accel_ref.z) >> 5); + booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_cmd[COMMAND_YAW] >> 16; + +} + + +/* + + generation of a saturated linear second order reference trajectory + + roll/pitch + omega : 1100 deg s-1 + zeta : 0.85 + max rotational accel : 128 rad.s-2 ( ~7300 deg s-2 ) + max rotational speed : 8 rad/s ( ~ 458 deg s-1 ) + + yaw + omega : 500 deg s-1 + zeta : 0.85 + max rotational accel : 32 rad.s-2 ( ~1833 deg s-2 ) + max rotational speed : 4 rad/s ( ~ 230 deg s-1 ) + + representation + accel : 20.12 + speed : 16.16 + angle : 12.20 + +*/ + +#define USE_REF 1 + +static inline void booz_stabilization_update_ref(void) { + +#ifdef USE_REF + BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE(); +#else + EULERS_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp); + INT_VECT3_ZERO(booz_stabilization_rate_ref); + INT_VECT3_ZERO(booz_stabilization_accel_ref); +#endif + + +} + + diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps_radio_control.c index 3d93d4f748..ca97cf91a7 100644 --- a/sw/simulator/nps_radio_control.c +++ b/sw/simulator/nps_radio_control.c @@ -32,6 +32,12 @@ void nps_radio_control_run_script(double time) { nps_radio_control.mode = MODE_SWITCH_AUTO2; // nps_radio_control.throttle = 0.265; // nps_radio_control.mode = MODE_SWITCH_MANUAL; + + if (((int32_t)rint((time*1.)))%2) + nps_radio_control.roll = 0.1; + else + nps_radio_control.roll = -0.1; + } }