From 2765d61d5985b2e002b80c79ad80dddc052db7a0 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Sat, 15 Aug 2009 18:19:22 +0000 Subject: [PATCH] switching to new supervision code for n-rotors --- .../booz/actuators/booz_actuators_asctec.c | 72 +++++++++++ .../booz_actuators_asctec.h} | 45 ++++--- .../booz/actuators/booz_actuators_mkk.c | 54 ++++++++ .../booz/actuators/booz_actuators_mkk.h | 53 ++++++++ sw/airborne/booz/actuators/booz_supervision.c | 65 ++++++++++ sw/airborne/booz/actuators/booz_supervision.h | 117 +---------------- .../booz/actuators/booz_supervision_old.h | 120 ++++++++++++++++++ .../arch/lpc21/actuators_asctec_twi_blmc_hw.c | 105 --------------- .../arch/lpc21/actuators_asctec_twi_blmc_hw.h | 110 ---------------- .../arch/lpc21/actuators_buss_twi_blmc_hw.h | 112 ---------------- sw/airborne/booz/booz2_autopilot.c | 13 -- sw/airborne/booz/booz2_autopilot.h | 50 +++++--- sw/airborne/booz/booz2_commands.c | 4 +- sw/airborne/booz/booz2_commands.h | 4 +- sw/airborne/booz/booz2_ins.c | 10 +- sw/airborne/booz/booz2_main.c | 14 +- sw/airborne/booz/booz_actuators.h | 13 ++ sw/airborne/booz/booz_ahrs.h | 4 +- sw/airborne/booz/{booz2_fms.c => booz_fms.c} | 55 ++++---- sw/airborne/booz/{booz2_fms.h => booz_fms.h} | 66 +++++----- sw/airborne/booz/fms/booz_fms_test_signal.c | 66 +++++----- sw/airborne/booz/fms/booz_fms_test_signal.h | 35 ++--- sw/airborne/booz/guidance/booz2_guidance_h.c | 8 +- sw/airborne/booz/guidance/booz2_guidance_v.c | 24 +++- sw/airborne/booz/guidance/booz2_guidance_v.h | 7 + .../booz/guidance/booz2_guidance_v_adpt.h | 2 +- .../booz_radio_control_spektrum_dx7se.h | 2 +- ...oz_stabilization_attitude_ref_quat_float.h | 5 +- 28 files changed, 612 insertions(+), 623 deletions(-) create mode 100644 sw/airborne/booz/actuators/booz_actuators_asctec.c rename sw/airborne/booz/{arch/lpc21/actuators_buss_twi_blmc_hw.c => actuators/booz_actuators_asctec.h} (56%) create mode 100644 sw/airborne/booz/actuators/booz_actuators_mkk.c create mode 100644 sw/airborne/booz/actuators/booz_actuators_mkk.h create mode 100644 sw/airborne/booz/actuators/booz_supervision.c create mode 100644 sw/airborne/booz/actuators/booz_supervision_old.h delete mode 100644 sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.c delete mode 100644 sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.h delete mode 100644 sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.h create mode 100644 sw/airborne/booz/booz_actuators.h rename sw/airborne/booz/{booz2_fms.c => booz_fms.c} (51%) rename sw/airborne/booz/{booz2_fms.h => booz_fms.h} (65%) diff --git a/sw/airborne/booz/actuators/booz_actuators_asctec.c b/sw/airborne/booz/actuators/booz_actuators_asctec.c new file mode 100644 index 0000000000..d4858f8f43 --- /dev/null +++ b/sw/airborne/booz/actuators/booz_actuators_asctec.c @@ -0,0 +1,72 @@ +#include "booz_actuators.h" +#include "actuators/booz_actuators_asctec.h" + +#include "booz2_commands.h" +#include "i2c.h" + +struct ActuatorsAsctec actuators_asctec; + +void actuators_init(void) { + actuators_asctec.cmd = NONE; + actuators_asctec.cur_addr = FRONT; + actuators_asctec.new_addr = FRONT; + actuators_asctec.i2c_done = TRUE; + actuators_asctec.nb_err = 0; +} + +void actuators_set(bool_t motors_on) { + if (!actuators_asctec.i2c_done) + actuators_asctec.nb_err++; + +#ifdef KILL_MOTORS + actuators_asctec.cmds[PITCH] = 0; + actuators_asctec.cmds[ROLL] = 0; + actuators_asctec.cmds[YAW] = 0; + actuators_asctec.cmds[THRUST] = 0; +#else /* ! KILL_MOTORS */ + actuators_asctec.cmds[PITCH] = booz2_commands[COMMAND_PITCH] + SUPERVISION_TRIM_E; + actuators_asctec.cmds[ROLL] = booz2_commands[COMMAND_ROLL] + SUPERVISION_TRIM_A; + actuators_asctec.cmds[YAW] = booz2_commands[COMMAND_YAW] + SUPERVISION_TRIM_R; + actuators_asctec.cmds[THRUST] = booz2_commands[COMMAND_THRUST]; + Bound(actuators_asctec.cmds[PITCH],-100, 100); + Bound(actuators_asctec.cmds[ROLL], -100, 100); + Bound(actuators_asctec.cmds[YAW], -100, 100); + if (motors_on) { + Bound(actuators_asctec.cmds[THRUST], 1, 200); + } + else + actuators_asctec.cmds[THRUST] = 0; +#endif /* KILL_MOTORS */ + + switch (actuators_asctec.cmd) { + case TEST: + i2c0_buf[0] = 251; + i2c0_buf[1] = actuators_asctec.cur_addr; + i2c0_buf[2] = 0; + i2c0_buf[3] = 231 + actuators_asctec.cur_addr; + break; + case REVERSE: + i2c0_buf[0] = 254; + i2c0_buf[1] = actuators_asctec.cur_addr; + i2c0_buf[2] = 0; + i2c0_buf[3] = 234 + actuators_asctec.cur_addr; + break; + case SET_ADDR: + i2c0_buf[0] = 250; + i2c0_buf[1] = actuators_asctec.cur_addr; + i2c0_buf[2] = actuators_asctec.new_addr; + i2c0_buf[3] = 230 + actuators_asctec.cur_addr + actuators_asctec.new_addr; + actuators_asctec.cur_addr = actuators_asctec.new_addr; + break; + case NONE: + i2c0_buf[0] = 100 - actuators_asctec.cmds[PITCH]; + i2c0_buf[1] = 100 + actuators_asctec.cmds[ROLL]; + i2c0_buf[2] = 100 - actuators_asctec.cmds[YAW]; + i2c0_buf[3] = actuators_asctec.cmds[THRUST]; + break; + } + actuators_asctec.cmd = NONE; + actuators_asctec.i2c_done = FALSE; + i2c0_transmit(0x02, 4, &actuators_asctec.i2c_done); +} + diff --git a/sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.c b/sw/airborne/booz/actuators/booz_actuators_asctec.h similarity index 56% rename from sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.c rename to sw/airborne/booz/actuators/booz_actuators_asctec.h index 446aa021a1..c4de69b4c4 100644 --- a/sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.c +++ b/sw/airborne/booz/actuators/booz_actuators_asctec.h @@ -21,24 +21,37 @@ * Boston, MA 02111-1307, USA. */ -#include "actuators.h" +#ifndef BOOZ_ACTUATORS_ASCTEC_H +#define BOOZ_ACTUATORS_ASCTEC_H -#include BOARD_CONFIG +enum actuators_astec_cmd { NONE, + TEST, + REVERSE, + SET_ADDR }; -uint8_t twi_blmc_nb_err; +enum actuators_astec_addr { FRONT, + BACK, + LEFT, + RIGHT }; -uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; -volatile bool_t buss_twi_blmc_status; -volatile bool_t buss_twi_blmc_i2c_done; -volatile uint8_t buss_twi_blmc_idx; -const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR; +enum actuators_astec_cmds { PITCH, + ROLL, + YAW, + THRUST, + CMD_NB }; -void actuators_init ( void ) { - uint8_t i; - for (i=0; i + * + * 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_actuators.h" +#include "actuators/booz_actuators_mkk.h" + +#include "booz2_commands.h" +#include "i2c.h" + +struct ActuatorsMkk actuators_mkk; + +const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR; + +void actuators_init(void) { + + supervision_init(); + actuators_mkk.status = IDLE; + actuators_mkk.i2c_done = TRUE; + actuators_mkk.idx = 0; + +} + + +void actuators_set(bool_t motors_on) { + + supervision_run(motors_on, booz2_commands); + actuators_mkk.status = BUSY; + actuators_mkk.i2c_done = FALSE; + actuators_mkk.idx = 0; + i2c0_buf[0] = supervision_commands[actuators_mkk.idx]; + i2c0_transmit(actuators_addr[actuators_mkk.idx], 1, &actuators_mkk.i2c_done); + +} + diff --git a/sw/airborne/booz/actuators/booz_actuators_mkk.h b/sw/airborne/booz/actuators/booz_actuators_mkk.h new file mode 100644 index 0000000000..d05c2fbd29 --- /dev/null +++ b/sw/airborne/booz/actuators/booz_actuators_mkk.h @@ -0,0 +1,53 @@ +/* + * $Id: actuators_buss_twi_blmc_hw.h 3847 2009-08-02 21:47:31Z 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. + */ + +#ifndef BOOZ_ACTUATORS_MKK_H +#define BOOZ_ACTUATORS_MKK_H + + +enum actuators_mkk_status {IDLE, BUSY}; + +struct ActuatorsMkk { + volatile enum actuators_mkk_status status; + volatile bool_t i2c_done; + volatile uint8_t idx; +}; + +extern struct ActuatorsMkk actuators_mkk; + + +#include "airframe.h" +#include "actuators/booz_supervision.h" +extern const uint8_t actuators_addr[]; + +#define ActuatorsMkkI2cHandler() { \ + actuators_mkk.idx++; \ + if (actuators_mkk.idx + * + * 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 "actuators/booz_supervision.h" + +#include "std.h" + +#include "airframe.h" + +static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF; +static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF; +static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF; + +int32_t supervision_commands[SUPERVISION_NB_MOTOR]; +static int32_t supervision_trim[SUPERVISION_NB_MOTOR]; + +void supervision_init(void) { + uint8_t i; + for (i=0; i - * - * 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 BOOZ_SUPERVISION_H #define BOOZ_SUPERVISION_H -#include "airframe.h" - -#if defined SUPERVISION_FRONT_ROTOR_CW -#define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) -#define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) -#define TRIM_BACK (-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) -#define TRIM_LEFT ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) -#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ - _mot_cmd[SERVO_FRONT] = _dt + _de - _dr + TRIM_FRONT; \ - _mot_cmd[SERVO_RIGHT] = _dt - _da + _dr + TRIM_RIGHT; \ - _mot_cmd[SERVO_BACK] = _dt - _de - _dr + TRIM_BACK; \ - _mot_cmd[SERVO_LEFT] = _dt + _da + _dr + TRIM_LEFT; \ - } -#else -#define TRIM_FRONT ( SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) -#define TRIM_RIGHT (-SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) -#define TRIM_BACK (-SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) -#define TRIM_LEFT ( SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) -#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ - _mot_cmd[SERVO_FRONT] = _dt + _de + _dr + TRIM_FRONT; \ - _mot_cmd[SERVO_RIGHT] = _dt - _da - _dr + TRIM_RIGHT; \ - _mot_cmd[SERVO_BACK] = _dt - _de + _dr + TRIM_BACK; \ - _mot_cmd[SERVO_LEFT] = _dt + _da - _dr + TRIM_LEFT; \ - } -#endif - -#define SUPERVISION_FIND_MAX_MOTOR(_mot_cmd, _max_mot) { \ - _max_mot = (-32767-1); /* INT16_MIN;*/ \ - if (_mot_cmd[SERVO_FRONT] > _max_mot) \ - max_mot = _mot_cmd[SERVO_FRONT]; \ - if (_mot_cmd[SERVO_RIGHT] > _max_mot) \ - max_mot = _mot_cmd[SERVO_RIGHT]; \ - if (_mot_cmd[SERVO_BACK] > _max_mot) \ - max_mot = _mot_cmd[SERVO_BACK]; \ - if (_mot_cmd[SERVO_LEFT] > _max_mot) \ - max_mot = _mot_cmd[SERVO_LEFT]; \ - } - -#define SUPERVISION_FIND_MIN_MOTOR(_mot_cmd, _min_mot) { \ - _min_mot = (32767); /*INT16_MAX;*/ \ - if (_mot_cmd[SERVO_FRONT] < _min_mot) \ - min_mot = _mot_cmd[SERVO_FRONT]; \ - if (_mot_cmd[SERVO_RIGHT] < _min_mot) \ - min_mot = _mot_cmd[SERVO_RIGHT]; \ - if (_mot_cmd[SERVO_BACK] < _min_mot) \ - min_mot = _mot_cmd[SERVO_BACK]; \ - if (_mot_cmd[SERVO_LEFT] < _min_mot) \ - min_mot = _mot_cmd[SERVO_LEFT]; \ - } - -#define SUPERVISION_OFFSET_MOTORS(_mot_cmd, _offset) { \ - _mot_cmd[SERVO_FRONT] += _offset; \ - _mot_cmd[SERVO_RIGHT] += _offset; \ - _mot_cmd[SERVO_BACK] += _offset; \ - _mot_cmd[SERVO_LEFT] += _offset; \ - } - -#define SUPERVISION_BOUND_MOTORS(_mot_cmd) { \ - Bound(_mot_cmd[SERVO_FRONT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ - Bound(_mot_cmd[SERVO_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ - Bound(_mot_cmd[SERVO_BACK] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ - Bound(_mot_cmd[SERVO_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ - } - - -#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) { \ - if (_motors_on) { \ - SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], _in[COMMAND_YAW], _in[COMMAND_THRUST]); \ - pprz_t min_mot; \ - SUPERVISION_FIND_MIN_MOTOR(_out, min_mot); \ - if (min_mot < SUPERVISION_MIN_MOTOR) { \ - pprz_t offset = -(min_mot - SUPERVISION_MIN_MOTOR); \ - SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ - } \ - pprz_t max_mot; \ - SUPERVISION_FIND_MAX_MOTOR(_out, max_mot); \ - if (max_mot > SUPERVISION_MAX_MOTOR) { \ - pprz_t offset = -(max_mot - SUPERVISION_MAX_MOTOR); \ - SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ - } \ - SUPERVISION_BOUND_MOTORS(_out); \ - } \ - else { \ - _out[SERVO_FRONT] = 0; \ - _out[SERVO_RIGHT] = 0; \ - _out[SERVO_BACK] = 0; \ - _out[SERVO_LEFT] = 0; \ - } \ - } - +#include "std.h" +extern void supervision_init(void); +extern void supervision_run(bool_t motors_on, int32_t in_cmd[]); +extern int32_t supervision_commands[]; #endif /* BOOZ_SUPERVISION_H */ diff --git a/sw/airborne/booz/actuators/booz_supervision_old.h b/sw/airborne/booz/actuators/booz_supervision_old.h new file mode 100644 index 0000000000..2278354136 --- /dev/null +++ b/sw/airborne/booz/actuators/booz_supervision_old.h @@ -0,0 +1,120 @@ +/* + * $Id$ + * + * 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. + */ + +#ifndef BOOZ_SUPERVISION_H +#define BOOZ_SUPERVISION_H + +#include "airframe.h" + +#if defined SUPERVISION_FRONT_ROTOR_CW +#define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) +#define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) +#define TRIM_BACK (-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) +#define TRIM_LEFT ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) +#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ + _mot_cmd[SERVO_FRONT] = _dt + _de - _dr + TRIM_FRONT; \ + _mot_cmd[SERVO_RIGHT] = _dt - _da + _dr + TRIM_RIGHT; \ + _mot_cmd[SERVO_BACK] = _dt - _de - _dr + TRIM_BACK; \ + _mot_cmd[SERVO_LEFT] = _dt + _da + _dr + TRIM_LEFT; \ + } +#else +#define TRIM_FRONT ( SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) +#define TRIM_RIGHT (-SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) +#define TRIM_BACK (-SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) +#define TRIM_LEFT ( SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) +#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ + _mot_cmd[SERVO_FRONT] = _dt + _de + _dr + TRIM_FRONT; \ + _mot_cmd[SERVO_RIGHT] = _dt - _da - _dr + TRIM_RIGHT; \ + _mot_cmd[SERVO_BACK] = _dt - _de + _dr + TRIM_BACK; \ + _mot_cmd[SERVO_LEFT] = _dt + _da - _dr + TRIM_LEFT; \ + } +#endif + +#define SUPERVISION_FIND_MAX_MOTOR(_mot_cmd, _max_mot) { \ + _max_mot = (-32767-1); /* INT16_MIN;*/ \ + if (_mot_cmd[SERVO_FRONT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_FRONT]; \ + if (_mot_cmd[SERVO_RIGHT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_RIGHT]; \ + if (_mot_cmd[SERVO_BACK] > _max_mot) \ + max_mot = _mot_cmd[SERVO_BACK]; \ + if (_mot_cmd[SERVO_LEFT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_LEFT]; \ + } + +#define SUPERVISION_FIND_MIN_MOTOR(_mot_cmd, _min_mot) { \ + _min_mot = (32767); /*INT16_MAX;*/ \ + if (_mot_cmd[SERVO_FRONT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_FRONT]; \ + if (_mot_cmd[SERVO_RIGHT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_RIGHT]; \ + if (_mot_cmd[SERVO_BACK] < _min_mot) \ + min_mot = _mot_cmd[SERVO_BACK]; \ + if (_mot_cmd[SERVO_LEFT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_LEFT]; \ + } + +#define SUPERVISION_OFFSET_MOTORS(_mot_cmd, _offset) { \ + _mot_cmd[SERVO_FRONT] += _offset; \ + _mot_cmd[SERVO_RIGHT] += _offset; \ + _mot_cmd[SERVO_BACK] += _offset; \ + _mot_cmd[SERVO_LEFT] += _offset; \ + } + +#define SUPERVISION_BOUND_MOTORS(_mot_cmd) { \ + Bound(_mot_cmd[SERVO_FRONT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_BACK] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + } + + +#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) { \ + if (_motors_on) { \ + SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], _in[COMMAND_YAW], _in[COMMAND_THRUST]); \ + pprz_t min_mot; \ + SUPERVISION_FIND_MIN_MOTOR(_out, min_mot); \ + if (min_mot < SUPERVISION_MIN_MOTOR) { \ + pprz_t offset = -(min_mot - SUPERVISION_MIN_MOTOR); \ + SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ + } \ + pprz_t max_mot; \ + SUPERVISION_FIND_MAX_MOTOR(_out, max_mot); \ + if (max_mot > SUPERVISION_MAX_MOTOR) { \ + pprz_t offset = -(max_mot - SUPERVISION_MAX_MOTOR); \ + SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ + } \ + SUPERVISION_BOUND_MOTORS(_out); \ + } \ + else { \ + _out[SERVO_FRONT] = 0; \ + _out[SERVO_RIGHT] = 0; \ + _out[SERVO_BACK] = 0; \ + _out[SERVO_LEFT] = 0; \ + } \ + } + + + + +#endif /* BOOZ_SUPERVISION_H */ diff --git a/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.c b/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.c deleted file mode 100644 index 10a51fed3f..0000000000 --- a/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * $Id$ - * - * 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 "actuators.h" - -#include "i2c.h" -#include "led.h" - -bool_t actuators_asctec_twi_blmc_command; -uint8_t actuators_asctec_twi_blmc_command_type; - -#define MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT 0 -#define MB_TWI_CONTROLLER_ASCTECH_ADDR_BACK 1 -#define MB_TWI_CONTROLLER_ASCTECH_ADDR_LEFT 2 -#define MB_TWI_CONTROLLER_ASCTECH_ADDR_RIGHT 3 -uint8_t actuators_asctec_twi_blmc_addr; -uint8_t actuators_asctec_twi_blmc_new_addr; - -#define ASCTEC_TWI_BLMC_NB 4 - -int8_t asctec_twi_blmc_motor_power[ASCTEC_TWI_BLMC_NB]; -uint8_t twi_blmc_nb_err; -volatile uint8_t mb_twi_i2c_done; - - -#define MB_TWI_CONTROLLER_MAX_CMD 200 -#define MB_TWI_CONTROLLER_ADDR 0x02 - -void actuators_init(void) { - twi_blmc_nb_err = 0; - mb_twi_i2c_done = TRUE; - actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE; - actuators_asctec_twi_blmc_addr = MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT; -} - -void asctec_twi_controller_send() { - if (!mb_twi_i2c_done) { - twi_blmc_nb_err++; - - } - if (actuators_asctec_twi_blmc_command != MB_TWI_CONTROLLER_COMMAND_NONE) { - - switch (actuators_asctec_twi_blmc_command) { - - case MB_TWI_CONTROLLER_COMMAND_TEST : - i2c0_buf[0] = 251; - i2c0_buf[1] = actuators_asctec_twi_blmc_addr; - i2c0_buf[2] = 0; - i2c0_buf[3] = 231 + actuators_asctec_twi_blmc_addr; - mb_twi_i2c_done = FALSE; - i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); - break; - - case MB_TWI_CONTROLLER_COMMAND_REVERSE : - i2c0_buf[0] = 254; - i2c0_buf[1] = actuators_asctec_twi_blmc_addr; - i2c0_buf[2] = 0; - i2c0_buf[3] = 234 + actuators_asctec_twi_blmc_addr; - mb_twi_i2c_done = FALSE; - i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); - break; - - case MB_TWI_CONTROLLER_COMMAND_SET_ADDR : - i2c0_buf[0] = 250; - i2c0_buf[1] = actuators_asctec_twi_blmc_addr; - i2c0_buf[2] = actuators_asctec_twi_blmc_new_addr; - i2c0_buf[3] = 230 + actuators_asctec_twi_blmc_addr + - actuators_asctec_twi_blmc_new_addr; - actuators_asctec_twi_blmc_addr = actuators_asctec_twi_blmc_new_addr; - mb_twi_i2c_done = FALSE; - i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); - break; - - } - actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE; - } - else { - i2c0_buf[0] = 100 + asctec_twi_blmc_motor_power[SERVO_PITCH]; - i2c0_buf[1] = 100 + asctec_twi_blmc_motor_power[SERVO_ROLL]; - i2c0_buf[2] = 100 + asctec_twi_blmc_motor_power[SERVO_YAW]; - i2c0_buf[3] = asctec_twi_blmc_motor_power[SERVO_THRUST]; - mb_twi_i2c_done = FALSE; - i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); - } -} diff --git a/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.h b/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.h deleted file mode 100644 index 04429eb06f..0000000000 --- a/sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * $Id$ - * - * 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. - */ - -#ifndef ACTUATORS_ASCTEC_TWI_BLMC_H -#define ACTUATORS_ASCTEC_TWI_BLMC_H - -#include "std.h" -#include "led.h" - -#include "airframe.h" - -extern void asctec_twi_controller_send(void); - -extern uint8_t twi_blmc_nb_err; -extern int8_t asctec_twi_blmc_motor_power[]; - -#define MB_TWI_CONTROLLER_COMMAND_NONE 0 -#define MB_TWI_CONTROLLER_COMMAND_TEST 1 -#define MB_TWI_CONTROLLER_COMMAND_REVERSE 2 -#define MB_TWI_CONTROLLER_COMMAND_SET_ADDR 3 - - -extern uint8_t actuators_asctec_twi_blmc_command; -extern uint8_t actuators_asctec_twi_blmc_addr; -extern uint8_t actuators_asctec_twi_blmc_new_addr; - -#define actuators_asctec_twi_blmc_hw_SetCommand(value) { \ - actuators_asctec_twi_blmc_command = value; \ - } - -#define actuators_asctec_twi_blmc_hw_SetAddr(value) { \ - actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_SET_ADDR; \ - actuators_asctec_twi_blmc_new_addr = value; \ - } - -#ifndef SetActuatorsFromCommands -#ifdef KILL_MOTORS -#define SetActuatorsFromCommands(_motors_on) { \ - Actuator(SERVO_PITCH) = 0; \ - Actuator(SERVO_ROLL) = 0; \ - Actuator(SERVO_YAW) = 0; \ - Actuator(SERVO_THRUST) = 0; \ - ActuatorsCommit(); \ - } -#else /* ! KILL_MOTORS */ -#ifndef SUPERVISION_HACK_45 -#define SetActuatorsFromCommands(_motors_on) { \ - booz2_commands[COMMAND_PITCH] += SUPERVISION_TRIM_E; \ - booz2_commands[COMMAND_ROLL] += SUPERVISION_TRIM_A; \ - booz2_commands[COMMAND_YAW] += SUPERVISION_TRIM_R; \ - Bound(booz2_commands[COMMAND_PITCH],-100, 100); \ - Bound(booz2_commands[COMMAND_ROLL], -100, 100); \ - Bound(booz2_commands[COMMAND_YAW], -100, 100); \ - if (_motors_on) { \ - Bound(booz2_commands[COMMAND_THRUST], 1, 200); \ - } \ - Actuator(SERVO_PITCH) = -(uint8_t)booz2_commands[COMMAND_PITCH]; \ - Actuator(SERVO_ROLL) = (uint8_t)booz2_commands[COMMAND_ROLL]; \ - Actuator(SERVO_YAW) = -(uint8_t)booz2_commands[COMMAND_YAW]; \ - Actuator(SERVO_THRUST) = (uint8_t)booz2_commands[COMMAND_THRUST]; \ - ActuatorsCommit(); \ - } -#else /* SUPERVISION_HACK_45 */ -#define SetActuatorsFromCommands(_motors_on) { \ - int32_t _r1 = booz2_commands[COMMAND_ROLL] + booz2_commands[COMMAND_PITCH]; \ - int32_t _p1 = booz2_commands[COMMAND_ROLL] - booz2_commands[COMMAND_PITCH]; \ - int32_t _y1 = booz2_commands[COMMAND_YAW]; \ - int32_t _t1 = booz2_commands[COMMAND_THRUST]; \ - Bound(_r1,-100, 100); \ - Bound(_p1,-100, 100); \ - Bound(_y1,-100, 100); \ - if (_motors_on) { \ - Bound(_t1, 1, 200); \ - } \ - Actuator(SERVO_ROLL) = _r1; \ - Actuator(SERVO_PITCH) = _p1; \ - Actuator(SERVO_YAW) = _y1; \ - Actuator(SERVO_THRUST) = _t1; \ - ActuatorsCommit(); \ - } -#endif /* SUPERVISION_HACK_45 */ -#endif /* KILL_MOTORS */ -#endif /* SetActuatorsFromCommands */ - -#define Actuator(i) asctec_twi_blmc_motor_power[i] -#define ActuatorsCommit() { \ - asctec_twi_controller_send(); \ - } - -#endif /* ACTUATORS_ASCTEC_TWI_BLMC_H */ diff --git a/sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.h b/sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.h deleted file mode 100644 index 35719f25c2..0000000000 --- a/sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * $Id$ - * - * 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. - */ - -#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H -#define ACTUATORS_BUSS_TWI_BLMC_HW_H - - - -#include -#include "std.h" -#include "i2c.h" - -#include "airframe.h" -#include "actuators/booz_supervision.h" - -/* - We're not using the airframe file "mixer" facility - and instead explicitely call the "supervision" code - to do the mixing -*/ -#ifndef SetActuatorsFromCommands -#ifdef KILL_MOTORS -#define SetActuatorsFromCommands(_motors_on) { \ - Actuator(SERVO_FRONT) = 0; \ - Actuator(SERVO_BACK) = 0; \ - Actuator(SERVO_RIGHT) = 0; \ - Actuator(SERVO_LEFT) = 0; \ - ActuatorsCommit(); \ - } -#else -#define SetActuatorsFromCommands(_motors_on) { \ - pprz_t mixed_commands[SERVOS_NB]; \ - BOOZ2_SUPERVISION_RUN(mixed_commands, booz2_commands, _motors_on); \ - Actuator(SERVO_FRONT) = (uint8_t)mixed_commands[SERVO_FRONT]; \ - Actuator(SERVO_BACK) = (uint8_t)mixed_commands[SERVO_BACK]; \ - Actuator(SERVO_RIGHT) = (uint8_t)mixed_commands[SERVO_RIGHT]; \ - Actuator(SERVO_LEFT) = (uint8_t)mixed_commands[SERVO_LEFT]; \ - ActuatorsCommit(); \ - } -#endif /* KILL_MOTORS */ -#endif /* SetActuatorsFromCommands */ - -#define ChopServo(x,a,b) ((x)>(b)?(b):(x)) -#define Actuator(i) buss_twi_blmc_motor_power[i] - -#ifdef TWI_IGNORE_ACK -#define ActuatorsCommit() { \ - buss_twi_blmc_idx = 0; \ - buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \ - ActuatorsBussTwiBlmcSend(); \ - } -#else -#define ActuatorsCommit() { \ - if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) { \ - buss_twi_blmc_idx = 0; \ - buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \ - ActuatorsBussTwiBlmcSend(); \ - } \ - else \ - twi_blmc_nb_err++; \ - } -#endif - -#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s)) - -#define BUSS_TWI_BLMC_STATUS_IDLE 0 -#define BUSS_TWI_BLMC_STATUS_BUSY 1 -#define BUSS_TWI_BLMC_NB 4 -//(sizeof(buss_twi_blmc_addr)/sizeof(uint8_t)) -extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; -extern volatile bool_t buss_twi_blmc_status; -extern uint8_t twi_blmc_nb_err; -extern volatile bool_t buss_twi_blmc_i2c_done; -extern volatile uint8_t buss_twi_blmc_idx; -extern const uint8_t buss_twi_blmc_addr[]; - -#define ActuatorsBussTwiBlmcNext() { \ - buss_twi_blmc_idx++; \ - if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) { \ - ActuatorsBussTwiBlmcSend(); \ - } \ - else \ - buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE; \ - } - -#define ActuatorsBussTwiBlmcSend() { \ - i2c0_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx]; \ - i2c0_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \ - } - - -#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */ diff --git a/sw/airborne/booz/booz2_autopilot.c b/sw/airborne/booz/booz2_autopilot.c index 9c7fed0f67..2352cef14a 100644 --- a/sw/airborne/booz/booz2_autopilot.c +++ b/sw/airborne/booz/booz2_autopilot.c @@ -57,12 +57,6 @@ void booz2_autopilot_init(void) { } -#if 0 -#include "uart.h" -#include "downlink.h" -#include "messages.h" -#endif - void booz2_autopilot_periodic(void) { RunOnceEvery(50, nav_periodic_task_10Hz()); @@ -228,13 +222,6 @@ void booz2_autopilot_set_mode(uint8_t new_autopilot_mode) { void booz2_autopilot_on_rc_frame(void) { -#if 0 - DOWNLINK_SEND_BOOZ_DEBUG(&rc_values[RADIO_THROTTLE], \ - &rc_values[RADIO_ROLL], \ - &rc_values[RADIO_PITCH], \ - &rc_values[RADIO_YAW]); -#endif - uint8_t new_autopilot_mode = 0; BOOZ_AP_MODE_OF_PPRZ(radio_control.values[RADIO_CONTROL_MODE], new_autopilot_mode); booz2_autopilot_set_mode(new_autopilot_mode); diff --git a/sw/airborne/booz/booz2_autopilot.h b/sw/airborne/booz/booz2_autopilot.h index 2cef6901ef..f580909fee 100644 --- a/sw/airborne/booz/booz2_autopilot.h +++ b/sw/airborne/booz/booz2_autopilot.h @@ -69,27 +69,47 @@ extern bool_t booz2_autopilot_detect_ground; #define TRESHOLD_1_PPRZ (MIN_PPRZ / 2) -#define TRESHOLD_2_PPRZ (MAX_PPRZ/2) +#define TRESHOLD_2_PPRZ (MAX_PPRZ / 2) #define BOOZ_AP_MODE_OF_PPRZ(_rc, _booz_mode) { \ - if (_rc > TRESHOLD_2_PPRZ) _booz_mode = booz2_autopilot_mode_auto2; \ - else if (_rc > TRESHOLD_1_PPRZ) _booz_mode = BOOZ2_MODE_AUTO1; \ - else _booz_mode = BOOZ2_MODE_MANUAL; \ + if (_rc > TRESHOLD_2_PPRZ) \ + _booz_mode = booz2_autopilot_mode_auto2; \ + else if (_rc > TRESHOLD_1_PPRZ) \ + _booz_mode = BOOZ2_MODE_AUTO1; \ + else \ + _booz_mode = BOOZ2_MODE_MANUAL; \ } -#define booz2_autopilot_KillThrottle(_v) { \ - kill_throttle = _v; \ - booz2_autopilot_motors_on = !kill_throttle; \ -} +#define booz2_autopilot_KillThrottle(_v) { \ + kill_throttle = _v; \ + booz2_autopilot_motors_on = !kill_throttle; \ + } + + +#define booz2_autopilot_SetTol(_v) { \ + booz2_autopilot_tol = _v; \ + if (_v == 1) { \ + booz_ins_vff_realign = TRUE; \ + booz2_guidance_v_z_sp = -POS_BFP_OF_REAL(2); \ + booz2_autopilot_mode_auto2 = BOOZ2_AP_MODE_ATTITUDE_Z_HOLD; \ + booz2_autopilot_set_mode(booz2_autopilot_mode_auto2); \ + } \ + if (_v == 2) { \ + booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); \ + booz2_autopilot_mode_auto2 = BOOZ2_AP_MODE_ATTITUDE_CLIMB; \ + booz2_autopilot_set_mode(booz2_autopilot_mode_auto2); \ + } \ + booz2_autopilot_tol = 0; \ + } #define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.) - -#define BoozDetectGroundEvent() { \ - if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE) { \ - if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) \ - booz2_autopilot_detect_ground = TRUE; \ - } \ -} +#define BoozDetectGroundEvent() { \ + if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE) { \ + if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \ + booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) \ + booz2_autopilot_detect_ground = TRUE; \ + } \ + } #endif /* BOOZ2_AUTOPILOT_H */ diff --git a/sw/airborne/booz/booz2_commands.c b/sw/airborne/booz/booz2_commands.c index d7d113b64e..b713652881 100644 --- a/sw/airborne/booz/booz2_commands.c +++ b/sw/airborne/booz/booz2_commands.c @@ -22,6 +22,6 @@ #include "booz2_commands.h" -pprz_t booz2_commands[COMMANDS_NB]; -const pprz_t booz2_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; +int32_t booz2_commands[COMMANDS_NB]; +const int32_t booz2_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; diff --git a/sw/airborne/booz/booz2_commands.h b/sw/airborne/booz/booz2_commands.h index 134db938bb..f898d3aef6 100644 --- a/sw/airborne/booz/booz2_commands.h +++ b/sw/airborne/booz/booz2_commands.h @@ -27,8 +27,8 @@ #include "paparazzi.h" #include "airframe.h" -extern pprz_t booz2_commands[COMMANDS_NB]; -extern const pprz_t booz2_commands_failsafe[COMMANDS_NB]; +extern int32_t booz2_commands[COMMANDS_NB]; +extern const int32_t booz2_commands_failsafe[COMMANDS_NB]; #define SetCommands(_in_cmd, _in_flight, _motors_on) { \ booz2_commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \ diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index 2afb318a39..7496d923d7 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -82,13 +82,15 @@ void booz_ins_propagate() { #ifdef USE_VFF if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING && booz_ins_baro_initialised) { -#if 0 +#ifdef BOOZ_INS_UNTILT_ACCEL + struct Int32Vect3 accel_body; + INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel); struct Int32Vect3 accel_ltp; - INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, booz_imu.accel); + INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body); float accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z); -#else +#else /* BOOZ_INS_UNTILT_ACCELS */ float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z); -#endif +#endif /* BOOZ_INS_UNTILT_ACCELS */ b2_vff_propagate(accel_float); booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot); booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot); diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c index 52ca67d9fa..686e62f706 100644 --- a/sw/airborne/booz/booz2_main.c +++ b/sw/airborne/booz/booz2_main.c @@ -33,19 +33,16 @@ #include "datalink.h" #include "booz2_commands.h" -#include ACTUATORS -//#include "booz2_servos_direct_hw.h" -//#include "booz2_control_surfaces.h" +#include "booz_actuators.h" #include "booz_radio_control.h" - #include "booz_imu.h" #include "booz2_gps.h" #include "booz2_analog_baro.h" #include "booz2_battery.h" -#include "booz2_fms.h" +#include "booz_fms.h" #include "booz2_autopilot.h" #include "booz_stabilization.h" @@ -82,9 +79,6 @@ STATIC_INLINE void booz2_main_init( void ) { hw_init(); sys_time_init(); actuators_init(); -#if defined USE_BOOZ2_SERVOS_DIRECT - booz2_servos_direct_init(); -#endif radio_control_init(); booz2_analog_init(); @@ -119,11 +113,11 @@ STATIC_INLINE void booz2_main_periodic( void ) { /* run control loops */ booz2_autopilot_periodic(); /* set actuators */ - SetActuatorsFromCommands(booz2_autopilot_motors_on); + actuators_set(booz2_autopilot_motors_on); PeriodicPrescaleBy10( \ { \ radio_control_periodic(); \ - if (radio_control.status != RADIO_CONTROL_OK && booz2_autopilot_mode != BOOZ2_AP_MODE_KILL) \ + if (radio_control.status != RADIO_CONTROL_OK && booz2_autopilot_mode != BOOZ2_AP_MODE_KILL)\ booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE); \ }, \ { \ diff --git a/sw/airborne/booz/booz_actuators.h b/sw/airborne/booz/booz_actuators.h new file mode 100644 index 0000000000..f61c4c62f9 --- /dev/null +++ b/sw/airborne/booz/booz_actuators.h @@ -0,0 +1,13 @@ +#ifndef BOOZ_ACTUATORS_H +#define BOZZ_ACTUATORS_H + +//#include ACTUATORS +//#include "booz2_servos_direct_hw.h" +//#include "booz2_control_surfaces.h" + +#include "std.h" + +extern void actuators_init(void); +extern void actuators_set(bool_t motors_on); + +#endif /* BOOZ_ACTUATORS_H */ diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h index f511d257e8..c2aa12a8f8 100644 --- a/sw/airborne/booz/booz_ahrs.h +++ b/sw/airborne/booz/booz_ahrs.h @@ -54,8 +54,8 @@ struct BoozAhrsFloat { extern struct BoozAhrs booz_ahrs; extern struct BoozAhrsFloat booz_ahrs_float; -#define BOOZ_AHRS_FLOAT_OF_INT32() { \ - QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \ +#define BOOZ_AHRS_FLOAT_OF_INT32() { \ + QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \ RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \ booz_ahrs_float.ltp_to_body_euler.phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi); \ booz_ahrs_float.ltp_to_body_euler.theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta); \ diff --git a/sw/airborne/booz/booz2_fms.c b/sw/airborne/booz/booz_fms.c similarity index 51% rename from sw/airborne/booz/booz2_fms.c rename to sw/airborne/booz/booz_fms.c index 6d7e39d6ae..c9871aa00f 100644 --- a/sw/airborne/booz/booz2_fms.c +++ b/sw/airborne/booz/booz_fms.c @@ -21,62 +21,59 @@ * Boston, MA 02111-1307, USA. */ -#include "booz2_fms.h" +#include "booz_fms.h" #include "booz_imu.h" #include "booz2_gps.h" #include "booz_ahrs.h" -bool_t booz_fms_on; -bool_t booz_fms_timeout; -uint8_t booz_fms_last_msg; -struct Booz_fms_info booz_fms_info; -struct Booz_fms_command booz_fms_input; +struct BoozFms fms; #define BOOZ_FMS_TIMEOUT 100 void booz_fms_init(void) { - booz_fms_on = FALSE; - booz_fms_timeout = TRUE; - booz_fms_last_msg = BOOZ_FMS_TIMEOUT; + fms.enabled = FALSE; + fms.timeouted = TRUE; + fms.last_msg = BOOZ_FMS_TIMEOUT; - booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude); - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; - booz_fms_input.v_sp.climb = 0; + fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + INT_EULERS_ZERO(fms.input.h_sp.attitude); + fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + fms.input.v_sp.climb = 0; booz_fms_impl_init(); + } void booz_fms_periodic(void) { - if (booz_fms_last_msg < BOOZ_FMS_TIMEOUT) - booz_fms_last_msg++; + if (fms.last_msg < BOOZ_FMS_TIMEOUT) + fms.last_msg++; else { - booz_fms_timeout = TRUE; - booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude); - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; - booz_fms_input.v_sp.climb = 0; + fms.timeouted = TRUE; + fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + INT_EULERS_ZERO(fms.input.h_sp.attitude); + fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + fms.input.v_sp.climb = 0; } booz_fms_impl_periodic(); } -void booz_fms_set_on_off(bool_t state __attribute__ ((unused))) { - +void booz_fms_set_enabled(bool_t enabled) { + booz_fms_impl_set_enabled(enabled); } void booz_fms_update_info(void) { // PPRZ_INT16_OF_INT32_RATES(booz_fms_info.imu.gyro, booz_imu.gyro); - PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu.accel); - PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, booz_imu.mag); + // PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu.accel); + //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, booz_imu.mag); - PPRZ_INT32_VECT3_COPY(booz_fms_info.gps.pos, booz_gps_state.ecef_pos); - PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.ecef_speed); - booz_fms_info.gps.pacc = booz_gps_state.pacc; - booz_fms_info.gps.num_sv = booz_gps_state.num_sv; - booz_fms_info.gps.fix = booz_gps_state.fix; + //PPRZ_INT32_VECT3_COPY(booz_fms_info.gps.pos, booz_gps_state.ecef_pos); + //PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.ecef_speed); + //booz_fms_info.gps.pacc = booz_gps_state.pacc; + //booz_fms_info.gps.num_sv = booz_gps_state.num_sv; + //booz_fms_info.gps.fix = booz_gps_state.fix; // PPRZ_INT16_OF_INT32_EULER(booz_fms_info.ahrs.euler, booz_ahrs_state.euler) // PPRZ_INT16_OF_INT32_RATE (booz_fms_info.ahrs.rate, booz_ahrs_state.rate) diff --git a/sw/airborne/booz/booz2_fms.h b/sw/airborne/booz/booz_fms.h similarity index 65% rename from sw/airborne/booz/booz2_fms.h rename to sw/airborne/booz/booz_fms.h index b674296ad2..d1d161f68e 100644 --- a/sw/airborne/booz/booz2_fms.h +++ b/sw/airborne/booz/booz_fms.h @@ -21,8 +21,8 @@ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef BOOZ2_FMS_H -#define BOOZ2_FMS_H +#ifndef BOOZ_FMS_H +#define BOOZ_FMS_H #include "std.h" #include "math/pprz_algebra_int.h" @@ -71,29 +71,37 @@ struct Booz_fms_command { uint8_t v_mode; }; -extern bool_t booz_fms_on; -extern bool_t booz_fms_timeout; -extern uint8_t booz_fms_last_msg; +struct BoozFms { + bool_t enabled; + bool_t timeouted; + uint8_t last_msg; + struct Booz_fms_info info; + struct Booz_fms_command input; +}; -extern struct Booz_fms_info booz_fms_info; -extern struct Booz_fms_command booz_fms_input; +extern struct BoozFms fms; extern void booz_fms_init(void); -extern void booz_fms_set_on_off(bool_t state); +extern void booz_fms_set_enabled(bool_t enabled); extern void booz_fms_periodic(void); extern void booz_fms_update_info(void); +/* must be implemented by specific module */ +extern void booz_fms_impl_init(void); +extern void booz_fms_impl_periodic(void); +extern void booz_fms_impl_set_enabled(bool_t enabled); -#define BOOZ2_FMS_TYPE_DATALINK 0 -#define BOOZ2_FMS_TYPE_TEST_SIGNAL 1 -#if defined BOOZ2_FMS_TYPE -#if BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_DATALINK -#include "booz2_fms_datalink.h" -#elif BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_TEST_SIGNAL -#include "booz2_fms_test_signal.h" +#define BOOZ_FMS_TYPE_DATALINK 0 +#define BOOZ_FMS_TYPE_TEST_SIGNAL 1 + +#if defined BOOZ_FMS_TYPE +#if BOOZ_FMS_TYPE == BOOZ_FMS_TYPE_DATALINK +#include "fms/booz_fms_datalink.h" +#elif BOOZ_FMS_TYPE == BOOZ_FMS_TYPE_TEST_SIGNAL +#include "fms/booz_fms_test_signal.h" #else -#error "booz2_fms.h: Unknown BOOZ2_FMS_TYPE" +#error "booz_fms.h: Unknown BOOZ2_FMS_TYPE" #endif #else /* no FMS */ #define booz_fms_init() {} @@ -101,22 +109,22 @@ extern void booz_fms_update_info(void); #endif #define BOOZ2_FMS_SET_POS_SP(_pos_sp,_psi_sp) { \ - _pos_sp.x = booz_fms_input.h_sp.pos.x; \ - _pos_sp.y = booz_fms_input.h_sp.pos.y; \ - /*_psi_sp = booz_fms_input.h_sp.pos.z;*/ \ + _pos_sp.x = fms.input.h_sp.pos.x; \ + _pos_sp.y = fms.input.h_sp.pos.y; \ + /*_psi_sp = fms.input.h_sp.pos.z;*/ \ } -#define BOOZ2_FMS_POS_INIT(_pos_sp,_psi_sp) { \ - booz_fms_input.h_sp.pos.x = _pos_sp.x; \ - booz_fms_input.h_sp.pos.y = _pos_sp.y; \ - booz_fms_input.h_sp.pos.z = _psi_sp; \ -} - -#define booz2_fms_SetOnOff(_val) { \ - booz_fms_on = _val; \ - booz_fms_set_on_off(booz_fms_on); \ +#define BOOZ2_FMS_POS_INIT(_pos_sp,_psi_sp) { \ + fms.input.h_sp.pos.x = _pos_sp.x; \ + fms.input.h_sp.pos.y = _pos_sp.y; \ + fms.input.h_sp.pos.z = _psi_sp; \ } -#endif /* BOOZ2_FMS_H */ +#define booz_fms_SetEnabled(_val) { \ + fms.enabled = _val; \ + booz_fms_set_enabled(_val); \ + } + +#endif /* BOOZ_FMS_H */ diff --git a/sw/airborne/booz/fms/booz_fms_test_signal.c b/sw/airborne/booz/fms/booz_fms_test_signal.c index 7c8c01fb52..e2ef096272 100644 --- a/sw/airborne/booz/fms/booz_fms_test_signal.c +++ b/sw/airborne/booz/fms/booz_fms_test_signal.c @@ -21,48 +21,49 @@ * Boston, MA 02111-1307, USA. */ -#include "booz2_fms.h" +#include "booz_fms.h" #include "booz2_ins.h" -#include "booz_geometry_mixed.h" +#include "math/pprz_algebra_int.h" -#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD 300; -#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE 20; -#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE 0 +#define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW +#define FMS_TEST_SIGNAL_DEFAULT_PERIOD 40 +#define FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE (((int32_t)ANGLE_BFP_OF_REAL(RadOfDeg(5)))<<8) -uint8_t booz_fms_test_signal_mode; - -uint32_t booz_fms_test_signal_period; -uint32_t booz_fms_test_signal_amplitude; -uint8_t booz_fms_test_signal_axe; -uint32_t booz_fms_test_signal_counter; - -uint32_t booz_fms_test_signal_start_z; +struct BoozFmsTestSignal fms_test_signal; void booz_fms_impl_init(void) { - booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE; - booz_fms_test_signal_period = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD; - booz_fms_test_signal_amplitude = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; - booz_fms_test_signal_axe = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE; - booz_fms_test_signal_counter = 0; - booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; + fms_test_signal.mode = FMS_TEST_SIGNAL_DEFAULT_MODE; + fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD; + fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; + fms_test_signal.counter = 0; + fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; } void booz_fms_impl_periodic(void) { - switch (booz_fms_test_signal_mode) { + switch (fms_test_signal.mode) { - case BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE: { - if (booz_fms_test_signal_counter < booz_fms_test_signal_period) { - PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, booz_fms_test_signal_amplitude, 0, 0); + case STEP_ROLL: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0); } else { - PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, -booz_fms_test_signal_amplitude, 0, 0); + EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0); } } break; - + case STEP_YAW: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude); + } + else { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude); + } + } + break; +#if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER) booz_fms_test_signal_start_z = booz_ins_ltp_pos.z; @@ -74,9 +75,14 @@ void booz_fms_impl_periodic(void) { } } break; - +#endif } - booz_fms_test_signal_counter++; - if (booz_fms_test_signal_counter >= 2 * booz_fms_test_signal_period) - booz_fms_test_signal_counter = 0; + fms_test_signal.counter++; + if (fms_test_signal.counter >= (2 * fms_test_signal.period)) + fms_test_signal.counter = 0; +} + +void booz_fms_impl_set_enabled(bool_t enabled) { + if (enabled) + fms_test_signal.counter = 0; } diff --git a/sw/airborne/booz/fms/booz_fms_test_signal.h b/sw/airborne/booz/fms/booz_fms_test_signal.h index ca54865641..7fd91e46a5 100644 --- a/sw/airborne/booz/fms/booz_fms_test_signal.h +++ b/sw/airborne/booz/fms/booz_fms_test_signal.h @@ -26,26 +26,31 @@ #include "std.h" -#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 0 -#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 1 -extern uint8_t booz_fms_test_signal_mode; +enum fms_ts_mode { + STEP_ROLL, + STEP_PITCH, + STEP_YAW, + STEP_VERT +}; -extern uint32_t booz_fms_test_signal_period; -extern uint32_t booz_fms_test_signal_amplitude; -extern uint8_t booz_fms_test_signal_axe; -extern uint32_t booz_fms_test_signal_counter; +struct BoozFmsTestSignal { + enum fms_ts_mode mode; + uint32_t period; + uint32_t amplitude; + uint32_t counter; +}; -extern void booz_fms_impl_init(void); -extern void booz_fms_impl_periodic(void); +extern struct BoozFmsTestSignal fms_test_signal; -#define booz2_fms_test_signal_SetPeriod(_val) { \ - booz_fms_test_signal_period = _val; \ - booz_fms_test_signal_counter = 0; \ -} +#define booz_fms_test_signal_SetPeriod(_val) { \ + fms_test_signal.period = _val; \ + fms_test_signal.counter = 0; \ + } -#define booz2_fms_test_signal_SetTsMode(_val) { \ - booz_fms_test_signal_mode = _val; \ +#define booz_fms_test_signal_SetMode(_val) { \ + fms_test_signal.mode = _val; \ + fms_test_signal.counter = 0; \ } #endif /* BOOZ_FMS_TEST_SIGNAL_H */ diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c index 09e3cde37c..c221bd5394 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_h.c +++ b/sw/airborne/booz/guidance/booz2_guidance_h.c @@ -25,7 +25,7 @@ #include "booz_ahrs.h" #include "booz_stabilization.h" -#include "booz2_fms.h" +#include "booz_fms.h" #include "booz2_ins.h" #include "booz2_navigation.h" @@ -111,14 +111,14 @@ void booz2_guidance_h_read_rc(bool_t in_flight) { case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: booz_stabilization_attitude_read_rc(in_flight); #ifdef USE_FMS - if (booz_fms_on) - BOOZ_STABILIZATION_ATTITUDE_ADD_SP(booz_fms_input.h_sp.attitude); + if (fms.enabled) + BOOZ_STABILIZATION_ATTITUDE_ADD_SP(fms.input.h_sp.attitude); #endif break; case BOOZ2_GUIDANCE_H_MODE_HOVER: #ifdef USE_FMS - if (booz_fms_on && booz_fms_input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER) + if (fms.enabled && fms.input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER) BOOZ2_FMS_SET_POS_SP(booz2_guidance_h_pos_sp,booz_stabilization_att_sp.psi); #endif BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight); diff --git a/sw/airborne/booz/guidance/booz2_guidance_v.c b/sw/airborne/booz/guidance/booz2_guidance_v.c index 1b5ca19cc6..121758a7da 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_v.c +++ b/sw/airborne/booz/guidance/booz2_guidance_v.c @@ -28,7 +28,7 @@ #include "booz_radio_control.h" #include "booz_stabilization.h" -#include "booz2_fms.h" +#include "booz_fms.h" #include "booz2_navigation.h" #include "booz2_ins.h" @@ -68,6 +68,7 @@ int32_t booz2_guidance_v_zdd_ref; int32_t booz2_guidance_v_kp; int32_t booz2_guidance_v_kd; +int32_t booz2_guidance_v_ki; int32_t booz2_guidance_v_z_sum_err; @@ -89,6 +90,7 @@ void booz2_guidance_v_init(void) { booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP; booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD; + booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI; booz2_guidance_v_z_sum_err = 0; @@ -170,8 +172,8 @@ void booz2_guidance_v_run(bool_t in_flight) { case BOOZ2_GUIDANCE_V_MODE_CLIMB: #ifdef USE_FMS - if (booz_fms_on && booz_fms_input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB) - booz2_guidance_v_zd_sp = booz_fms_input.v_sp.climb; + if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB) + booz2_guidance_v_zd_sp = fms.input.v_sp.climb; #endif b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp); run_hover_loop(in_flight); @@ -181,8 +183,8 @@ void booz2_guidance_v_run(bool_t in_flight) { case BOOZ2_GUIDANCE_V_MODE_HOVER: #ifdef USE_FMS - if (booz_fms_on && booz_fms_input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER) - booz2_guidance_v_z_sp = booz_fms_input.v_sp.height; + if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER) + booz2_guidance_v_z_sp = fms.input.v_sp.height; #endif b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp); run_hover_loop(in_flight); @@ -228,8 +230,10 @@ static inline void run_hover_loop(bool_t in_flight) { int32_t err_zd = booz_ins_ltp_speed.z - booz2_guidance_v_zd_ref; Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD); - if (in_flight) + if (in_flight) { booz2_guidance_v_z_sum_err += err_z; + Bound(booz2_guidance_v_z_sum_err, -BOOZ2_GUIDANCE_V_MAX_SUM_ERR, BOOZ2_GUIDANCE_V_MAX_SUM_ERR); + } else booz2_guidance_v_z_sum_err = 0; @@ -241,17 +245,23 @@ static inline void run_hover_loop(bool_t in_flight) { #endif const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) - (booz2_guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC)); +#if 0 if (g_m_zdd > 0) booz2_guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m; else booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m; +#else + booz2_guidance_v_ff_cmd = g_m_zdd / inv_m; +#endif // booz2_guidance_v_ff_cmd = BOOZ2_GUIDANCE_V_HOVER_POWER; /* our error command */ booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) + - ((-booz2_guidance_v_kd * err_zd) >> 21); + ((-booz2_guidance_v_kd * err_zd) >> 21) + + ((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 21); booz2_guidance_v_delta_t = booz2_guidance_v_ff_cmd + booz2_guidance_v_fb_cmd; + // booz2_guidance_v_delta_t = booz2_guidance_v_fb_cmd; } diff --git a/sw/airborne/booz/guidance/booz2_guidance_v.h b/sw/airborne/booz/guidance/booz2_guidance_v.h index a9b3de5083..88b286fa22 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_v.h +++ b/sw/airborne/booz/guidance/booz2_guidance_v.h @@ -50,6 +50,7 @@ extern int32_t booz2_guidance_v_delta_t; extern int32_t booz2_guidance_v_kp; extern int32_t booz2_guidance_v_kd; +extern int32_t booz2_guidance_v_ki; extern void booz2_guidance_v_init(void); extern void booz2_guidance_v_read_rc(void); @@ -57,4 +58,10 @@ extern void booz2_guidance_v_mode_changed(uint8_t new_mode); extern void booz2_guidance_v_notify_in_flight(bool_t in_flight); extern void booz2_guidance_v_run(bool_t in_flight); +#define booz2_guidance_v_SetKi(_val) { \ + booz2_guidance_v_ki = _val; \ + booz2_guidance_v_z_sum_err = 0; \ + } + + #endif /* BOOZ2_GUIDANCE_V */ diff --git a/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h b/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h index 055061af57..a68899eef9 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h +++ b/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h @@ -64,7 +64,7 @@ int32_t b2_gv_adapt_Xmeas; /* System and Measuremement noises */ #define B2_GV_ADAPT_SYS_NOISE_F 0.00005 #define B2_GV_ADAPT_SYS_NOISE BFP_OF_REAL(B2_GV_ADAPT_SYS_NOISE_F, B2_GV_ADAPT_P_FRAC) -#define B2_GV_ADAPT_MEAS_NOISE_F 3.0 +#define B2_GV_ADAPT_MEAS_NOISE_F 2.0 #define B2_GV_ADAPT_MEAS_NOISE BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_F, B2_GV_ADAPT_P_FRAC) diff --git a/sw/airborne/booz/radio_control/booz_radio_control_spektrum_dx7se.h b/sw/airborne/booz/radio_control/booz_radio_control_spektrum_dx7se.h index 5c8c772e13..4ce1420b35 100644 --- a/sw/airborne/booz/radio_control/booz_radio_control_spektrum_dx7se.h +++ b/sw/airborne/booz/radio_control/booz_radio_control_spektrum_dx7se.h @@ -38,7 +38,7 @@ -MAX_PPRZ/MAX_SPK, \ MAX_PPRZ/MAX_SPK, \ MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ MAX_PPRZ/MAX_SPK } /* diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h index 3742ec8aa4..62f0905f40 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h @@ -65,10 +65,9 @@ booz_stab_att_ref_euler.psi = _sp.psi; \ booz_stab_att_ref_rate.r = 0; \ struct FloatRMat sp_rmat; \ - /*FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp);*/ \ - FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp); \ + FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); \ + /* FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/ \ FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat); \ - /*FLOAT_EULERS_OF_QUAT(sp_euler321, sp_quat);*/ \ }