Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

switching to new supervision code for n-rotors

Browse Source
This commit is contained in:
Antoine Drouin
2009-08-15 18:19:22 +00:00
parent 34cbd3eebe
commit 2765d61d59
28 changed files with 612 additions and 623 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/actuators/booz_actuators_asctec.c
+72
View File
@@ -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);
}
sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.c → sw/airborne/booz/actuators/booz_actuators_asctec.h
+29 -16
View File
@@ -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<BUSS_TWI_BLMC_NB;i++)
buss_twi_blmc_motor_power[i] = 0;
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
twi_blmc_nb_err = 0;
buss_twi_blmc_i2c_done = TRUE;
}
struct ActuatorsAsctec {
enum actuators_astec_cmd cmd;
enum actuators_astec_addr cur_addr;
enum actuators_astec_addr new_addr;
int32_t cmds[CMD_NB];
volatile bool_t i2c_done;
volatile uint32_t nb_err;
};
extern struct ActuatorsAsctec actuators_asctec;
#endif /* BOOZ_ACTUATORS_ASCTEC_H */
sw/airborne/booz/actuators/booz_actuators_mkk.c
+54
View File
@@ -0,0 +1,54 @@
/*
* $Id: actuators_buss_twi_blmc_hw.h 3847 2009-08-02 21:47:31Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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);
}
sw/airborne/booz/actuators/booz_actuators_mkk.h
+53
View File
@@ -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 <poinix@gmail.com>
*
* 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<ACTUATORS_MKK_NB) { \
i2c0_buf[0] = supervision_commands[actuators_mkk.idx]; \
i2c0_transmit(actuators_addr[actuators_mkk.idx], 1, &actuators_mkk.i2c_done); \
} \
else \
actuators_mkk.status = IDLE; \
}
#endif /* BOOZ_ACTUATORS_MKK_H */
sw/airborne/booz/actuators/booz_supervision.c
+65
View File
@@ -0,0 +1,65 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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<SUPERVISION_NB_MOTOR; i++) {
supervision_commands[i] = 0;
supervision_trim[i] =
roll_coef[i] * SUPERVISION_TRIM_A +
pitch_coef[i] * SUPERVISION_TRIM_E +
yaw_coef[i] * SUPERVISION_TRIM_R;
}
}
void supervision_run(bool_t motors_on, int32_t in_cmd[] ) {
uint8_t i;
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
if (motors_on) {
supervision_commands[i] =
in_cmd[COMMAND_THRUST] +
(roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] +
supervision_trim[i] )/256;
Bound(supervision_commands[i], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
}
else
supervision_commands[i] = 0;
}
}
sw/airborne/booz/actuators/booz_supervision.h
+4 -113
View File
@@ -1,120 +1,11 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/booz/actuators/booz_supervision_old.h
+120
View File
@@ -0,0 +1,120 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.c
-105
View File
@@ -1,105 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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);
}
}
sw/airborne/booz/arch/lpc21/actuators_asctec_twi_blmc_hw.h
-110
View File
@@ -1,110 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 */
sw/airborne/booz/arch/lpc21/actuators_buss_twi_blmc_hw.h
-112
View File
@@ -1,112 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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 <string.h>
#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 */
sw/airborne/booz/booz2_autopilot.c
-13
View File
@@ -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);
sw/airborne/booz/booz2_autopilot.h
+35 -15
View File
@@ -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 */
sw/airborne/booz/booz2_commands.c
+2 -2
View File
@@ -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;
sw/airborne/booz/booz2_commands.h
+2 -2
View File
@@ -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]; \
sw/airborne/booz/booz2_ins.c
+6 -4
View File
@@ -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);
sw/airborne/booz/booz2_main.c
+4 -10
View File
@@ -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); \
}, \
{ \
sw/airborne/booz/booz_actuators.h
+13
View File
@@ -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 */
sw/airborne/booz/booz_ahrs.h
+2 -2
View File
@@ -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); \
sw/airborne/booz/booz2_fms.c → sw/airborne/booz/booz_fms.c
+26 -29
View File
@@ -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)
sw/airborne/booz/booz2_fms.h → sw/airborne/booz/booz_fms.h
+37 -29
View File
@@ -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 */
sw/airborne/booz/fms/booz_fms_test_signal.c
+36 -30
View File
@@ -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;
}
sw/airborne/booz/fms/booz_fms_test_signal.h
+20 -15
View File
@@ -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 */
sw/airborne/booz/guidance/booz2_guidance_h.c
+4 -4
View File
@@ -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);
sw/airborne/booz/guidance/booz2_guidance_v.c
+17 -7
View File
@@ -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;
}
sw/airborne/booz/guidance/booz2_guidance_v.h
+7
View File
@@ -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 */
sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
+1 -1
View File
@@ -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)
sw/airborne/booz/radio_control/booz_radio_control_spektrum_dx7se.h
+1 -1
View File
@@ -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 }
/*
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
+2 -3
View File
@@ -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);*/ \
}