Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-29 16:55:51 +00:00
parent b516fe1bea
commit bde0de08c3
18 changed files with 531 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/conf_booz.makefile
+88
View File
@@ -0,0 +1,88 @@
ARCHI=arm7
FLASH_MODE = IAP
#
# controller CPU
#
ctl.ARCHDIR = $(ARCHI)
ctl.ARCH = arm7tdmi
ctl.TARGET = ctl
ctl.TARGETDIR = ctl
ctl.CFLAGS += -DBOOZ_CONTROLLER_MCU -DCONFIG=\"conf_booz.h\"
ctl.srcs = booz_controller_main.c
ctl.CFLAGS += -DLED
ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' -DTIME_LED=1
ctl.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
ctl.srcs += $(SRC_ARCH)/armVIC.c
ctl.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
ctl.srcs += $(SRC_ARCH)/uart_hw.c
ctl.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
ctl.srcs += booz_telemetry.c downlink.c pprz_transport.c
ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
ctl.srcs += booz_datalink.c
#ctl.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING
#ctl.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
ctl.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
ctl.srcs += $(SRC_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
ctl.CFLAGS += -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
ctl.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=4
ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ctl.srcs += booz_inter_mcu.c
ctl.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
ctl.CFLAGS += -DSPI_SELECT_SLAVE0_PIN=20 -DSPI_SELECT_SLAVE0_PORT=0 -DSSPCPSR_VAL=0x0C
ctl.srcs += booz_link_mcu.c $(SRC_ARCH)/booz_link_mcu_hw.c
ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c
ctl.srcs += commands.c
ctl.CFLAGS += -DDISABLE_NAV
ctl.srcs += booz_estimator.c \
booz_control.c \
booz_nav.c \
booz_autopilot.c
#
# FILTER CPU
#
flt.ARCHDIR = $(ARCHI)
flt.ARCH = arm7tdmi
flt.TARGET = flt
flt.TARGETDIR = flt
flt.CFLAGS += -DBOOZ_FILTER_MCU -DCONFIG=\"pprz_imu.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
flt.srcs = booz_filter_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
flt.CFLAGS += -DLED
flt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
flt.srcs += $(SRC_ARCH)/uart_hw.c
flt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
flt.srcs += downlink.c pprz_transport.c booz_filter_telemetry.c
flt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
flt.srcs += $(SRC_ARCH)/adc_hw.c
flt.srcs += max1167.c $(SRC_ARCH)/max1167_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
flt.srcs += multitilt.c
flt.srcs += booz_inter_mcu.c
flt.srcs += booz_link_mcu.c $(SRC_ARCH)/booz_link_mcu_hw.c
conf/messages.xml
+34 -19
View File
@@ -429,22 +429,40 @@
</message>
<message name="BOOZ_RATE_LOOP" ID="233">
<field name="m_p" type="float"/>
<field name="sp_p" type="float"/>
<field name="m_q" type="float"/>
<field name="sp_q" type="float"/>
<field name="m_r" type="float"/>
<field name="sp_r" type="float"/>
<field name="est_p" type="float"/>
<field name="sp_p" type="float"/>
<field name="est_q" type="float"/>
<field name="sp_q" type="float"/>
<field name="est_r" type="float"/>
<field name="sp_r" type="float"/>
</message>
<message name="BOOZ_ATT_LOOP" ID="234">
<field name="m_phi" type="float"/>
<field name="est_phi" type="float"/>
<field name="sp_phi" type="float"/>
<field name="m_theta" type="float"/>
<field name="est_theta" type="float"/>
<field name="sp_theta" type="float"/>
</message>
<message name="BOOZ_CMDS" ID="236">
<message name="BOOZ_VERT_LOOP" ID="235">
<field name="sp_z" type="float"/>
<field name="est_vz" type="float"/>
<field name="est_z" type="float"/>
<field name="power_command" type="float"/>
</message>
<message name="BOOZ_HOV_LOOP" ID="236">
<field name="sp_x" type="float"/>
<field name="sp_y" type="float"/>
<field name="est_vx" type="float"/>
<field name="est_x" type="float"/>
<field name="est_vy" type="float"/>
<field name="est_y" type="float"/>
<field name="phi_command" type="float"/>
<field name="theta_command" type="float"/>
</message>
<message name="BOOZ_CMDS" ID="237">
<field name="front" type="uint8"/>
<field name="back" type="uint8"/>
<field name="left" type="uint8"/>
@@ -452,35 +470,32 @@
</message>
<message name="BOOZ_UF_RATES" ID="237">
<message name="BOOZ_UF_RATES" ID="238">
<field name="p" type="float"/>
<field name="q" type="float"/>
<field name="r" type="float"/>
</message>
<message name="BOOZ_SIM_RPMS" ID="235">
<message name="BOOZ_SIM_RPMS" ID="239">
<field name="front" type="float" unit="RPM"/>
<field name="back" type="float" unit="RPM"/>
<field name="left" type="float" unit="RPM"/>
<field name="right" type="float" unit="RPM"/>
</message>
<message name="BOOZ_SIM_POS_SPEED" ID="238">
<field name="x" type="float"/>
<field name="y" type="float"/>
<field name="z" type="float"/>
<message name="BOOZ_SIM_SPEED_POS" ID="240">
<field name="u" type="float"/>
<field name="v" type="float"/>
<field name="w" type="float"/>
<field name="x" type="float"/>
<field name="y" type="float"/>
<field name="z" type="float"/>
</message>
<message name="BOOZ_SIM_ATTITUDE" ID="239">
<message name="BOOZ_SIM_ATTITUDE_RATE" ID="241">
<field name="phi" type="float" unit="degres"/>
<field name="theta" type="float" unit="degres"/>
<field name="psi" type="float" unit="degres"/>
</message>
<message name="BOOZ_SIM_BODY_RATE" ID="240">
<field name="p" type="float" unit="degres"/>
<field name="q" type="float" unit="degres"/>
<field name="r" type="float" unit="degres"/>
conf/telemetry/booz.xml
+3 -1
View File
@@ -9,8 +9,10 @@
<!-- <message name="BOOZ_FD" period="0.05"/> -->
<!-- <message name="BOOZ_DEBUG" period="0.25"/> -->
<message name="ACTUATORS" period="0.5"/>
<message name="BOOZ_RATE_LOOP" period="0.05"/>
<message name="BOOZ_HOV_LOOP" period="0.05"/>
<message name="BOOZ_VERT_LOOP" period="0.05"/>
<message name="BOOZ_ATT_LOOP" period="0.05"/>
<message name="BOOZ_RATE_LOOP" period="0.05"/>
<message name="BOOZ_CMDS" period="0.05"/>
<message name="BOOZ_UF_RATES" period="0.05"/>
<message name="DL_VALUE" period="1."/>
sw/airborne/booz_autopilot.c
+10 -4
View File
@@ -3,6 +3,7 @@
#include "radio_control.h"
#include "commands.h"
#include "booz_control.h"
#include "booz_nav.h"
uint8_t booz_autopilot_mode;
@@ -14,6 +15,7 @@ void booz_autopilot_periodic_task(void) {
switch (booz_autopilot_mode) {
case BOOZ_AP_MODE_FAILSAFE:
case BOOZ_AP_MODE_KILL:
SetCommands(commands_failsafe);
break;
case BOOZ_AP_MODE_RATE:
@@ -25,7 +27,7 @@ void booz_autopilot_periodic_task(void) {
SetCommands(booz_control_commands);
break;
case BOOZ_AP_MODE_NAV:
booz_control_nav_run();
booz_nav_run();
SetCommands(booz_control_commands);
break;
}
@@ -34,16 +36,20 @@ void booz_autopilot_periodic_task(void) {
void booz_autopilot_on_rc_event(void) {
/* I think this should be hidden in rc code */
/* the ap gets a mode everytime - the rc filters it */
if (rc_values_contains_avg_channels) {
booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
}
switch (booz_autopilot_mode) {
case BOOZ_AP_MODE_RATE:
booz_control_rate_compute_setpoints();
booz_control_rate_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_ATTITUDE:
booz_control_attitude_compute_setpoints();
booz_control_attitude_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_NAV:
booz_nav_read_setpoints_from_rc();
break;
}
}
sw/airborne/booz_autopilot.h
+4 -3
View File
@@ -4,9 +4,10 @@
#include "std.h"
#define BOOZ_AP_MODE_FAILSAFE 0
#define BOOZ_AP_MODE_RATE 1
#define BOOZ_AP_MODE_ATTITUDE 2
#define BOOZ_AP_MODE_NAV 3
#define BOOZ_AP_MODE_KILL 1
#define BOOZ_AP_MODE_RATE 2
#define BOOZ_AP_MODE_ATTITUDE 3
#define BOOZ_AP_MODE_NAV 4
extern uint8_t booz_autopilot_mode;
sw/airborne/booz_control.c
+12 -27
View File
@@ -29,8 +29,8 @@ pprz_t booz_control_commands[COMMANDS_NB];
#define BOOZ_CONTROL_RATE_R_MAX_SP 100.
float booz_control_phi_sp;
float booz_control_theta_sp;
float booz_control_attitude_phi_sp;
float booz_control_attitude_theta_sp;
float booz_control_attitude_phi_theta_pgain;
float booz_control_attitude_phi_theta_dgain;
@@ -59,15 +59,15 @@ void booz_control_init(void) {
booz_control_rate_r_dgain = BOOZ_CONTROL_RATE_R_DGAIN;
booz_control_phi_sp = 0.;
booz_control_theta_sp =0.;
booz_control_attitude_phi_sp = 0.;
booz_control_attitude_theta_sp =0.;
booz_control_attitude_phi_theta_pgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN;
booz_control_attitude_phi_theta_dgain = BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN;
}
void booz_control_rate_compute_setpoints(void) {
void booz_control_rate_read_setpoints_from_rc(void) {
booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
@@ -101,10 +101,12 @@ void booz_control_rate_run(void) {
}
void booz_control_attitude_compute_setpoints(void) {
void booz_control_attitude_read_setpoints_from_rc(void) {
booz_control_phi_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_theta_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_phi_sp = -rc_values[RADIO_ROLL] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
@@ -112,11 +114,11 @@ void booz_control_attitude_compute_setpoints(void) {
void booz_control_attitude_run(void) {
const float att_err_phi = booz_estimator_phi - booz_control_phi_sp;
const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi +
booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
const float att_err_theta = booz_estimator_theta - booz_control_theta_sp;
const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta +
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
@@ -132,20 +134,3 @@ void booz_control_attitude_run(void) {
}
void booz_control_nav_compute_setpoints(void) {
}
void booz_control_nav_run(void) {
#if 0
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
#else
booz_control_rate_run();
#endif
}
sw/airborne/booz_control.h
+10 -10
View File
@@ -6,14 +6,12 @@
extern void booz_control_init(void);
extern void booz_control_rate_compute_setpoints(void);
extern void booz_control_rate_read_setpoints_from_rc(void);
extern void booz_control_rate_run(void);
extern void booz_control_attitude_compute_setpoints(void);
extern void booz_control_attitude_read_setpoints_from_rc(void);
extern void booz_control_attitude_run(void);
extern void booz_control_nav_run(void);
extern float booz_control_p_sp;
extern float booz_control_q_sp;
extern float booz_control_r_sp;
@@ -24,16 +22,18 @@ extern float booz_control_rate_pq_dgain;
extern float booz_control_rate_r_pgain;
extern float booz_control_rate_r_dgain;
extern pprz_t booz_control_command_p;
extern pprz_t booz_control_command_q;
extern pprz_t booz_control_command_r;
extern float booz_control_attitude_phi_sp;
extern float booz_control_attitude_theta_sp;
extern float booz_control_phi_sp;
extern float booz_control_theta_sp;
extern float booz_control_attitude_phi_theta_pgain;
extern float booz_control_attitude_phi_theta_dgain;
#define BoozControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _r_sp, _power_sp) { \
booz_control_attitude_phi_sp = _phi_sp; \
booz_control_attitude_theta_sp = _theta_sp; \
booz_control_r_sp = _r_sp; \
booz_control_power_sp = _power_sp; \
}
#include "airframe.h"
extern pprz_t booz_control_commands[COMMANDS_NB];
sw/airborne/booz_controller_main.c
+2
View File
@@ -16,6 +16,7 @@
#include "booz_estimator.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "booz_autopilot.h"
#include "uart.h"
@@ -61,6 +62,7 @@ STATIC_INLINE void booz_controller_main_init( void ) {
booz_estimator_init();
booz_control_init();
booz_nav_init();
booz_autopilot_init();
//FIXME
sw/airborne/booz_estimator.c
+64 -9
View File
@@ -15,6 +15,18 @@ float booz_estimator_phi;
float booz_estimator_theta;
float booz_estimator_psi;
#ifndef DISABLE_NAV
float booz_estimator_dcm[AXIS_NB][AXIS_NB];
float booz_estimator_x;
float booz_estimator_y;
float booz_estimator_z;
float booz_estimator_vx;
float booz_estimator_vy;
float booz_estimator_vz;
#endif /* DISABLE_NAV */
void booz_estimator_init( void ) {
booz_estimator_uf_p = 0.;
@@ -28,19 +40,62 @@ void booz_estimator_init( void ) {
booz_estimator_phi = 0.;
booz_estimator_theta = 0.;
booz_estimator_psi = 0.;
#ifndef DISABLE_NAV
booz_estimator_x = 0.;
booz_estimator_y = 0.;
booz_estimator_z = 0.;
booz_estimator_vx = 0.;
booz_estimator_vy = 0.;
booz_estimator_vz = 0.;
#endif /* DISABLE_NAV */
}
void booz_estimator_read_inter_mcu_state( void ) {
booz_estimator_uf_p = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_uf_q = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_uf_r = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_p = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_q = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_r = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_phi = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
booz_estimator_psi = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
booz_estimator_uf_p = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_uf_q = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_uf_r = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_p = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_q = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_r = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_phi = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
booz_estimator_psi = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
}
void booz_estimator_compute_dcm( void ) {
float sinPHI = sin( booz_estimator_phi );
float cosPHI = cos( booz_estimator_phi );
float sinTHETA = sin( booz_estimator_theta);
float cosTHETA = cos( booz_estimator_theta);
float sinPSI = sin( booz_estimator_psi);
float cosPSI = cos( booz_estimator_psi);
booz_estimator_dcm[0][0] = cosTHETA * cosPSI;
booz_estimator_dcm[0][1] = cosTHETA * sinPSI;
booz_estimator_dcm[0][2] = -sinTHETA;
booz_estimator_dcm[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI;
booz_estimator_dcm[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI;
booz_estimator_dcm[1][2] = sinPHI * cosTHETA;
booz_estimator_dcm[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI;
booz_estimator_dcm[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI;
booz_estimator_dcm[2][2] = cosPHI * cosTHETA;
}
#ifndef DISABLE_NAV
void booz_estimator_set_speed_and_pos(float _vx, float _vy, float _vz, float _x, float _y, float _z) {
booz_estimator_vx = _vx;
booz_estimator_vy = _vy;
booz_estimator_vz = _vz;
booz_estimator_x = _x;
booz_estimator_y = _y;
booz_estimator_z = _z;
}
#endif /* DISABLE_NAV */
sw/airborne/booz_estimator.h
+25
View File
@@ -1,6 +1,9 @@
#ifndef BOOZ_ESTIMATOR_H
#define BOOZ_ESTIMATOR_H
#include "6dof.h"
/* unfiltered rates available when filter crashed or uninitialed */
extern float booz_estimator_uf_p;
extern float booz_estimator_uf_q;
extern float booz_estimator_uf_r;
@@ -13,7 +16,29 @@ extern float booz_estimator_phi;
extern float booz_estimator_theta;
extern float booz_estimator_psi;
#ifndef DISABLE_NAV
extern float booz_estimator_dcm[AXIS_NB][AXIS_NB];
/* position in earth frame : not yet available - sim only */
extern float booz_estimator_x;
extern float booz_estimator_y;
extern float booz_estimator_z;
/* speed in earth frame : not yet available - sim only */
extern float booz_estimator_vx;
extern float booz_estimator_vy;
extern float booz_estimator_vz;
#endif /* DISABLE_NAV */
extern void booz_estimator_init( void );
extern void booz_estimator_read_inter_mcu_state( void );
#ifndef DISABLE_NAV
extern void booz_estimator_compute_dcm( void );
extern void booz_estimator_set_speed_and_pos(float _vx, float _vy, float _vz, float _x, float _y, float _z);
#endif
#endif /* BOOZ_ESTIMATOR_H */
sw/airborne/booz_nav.c
+99
View File
@@ -0,0 +1,99 @@
#include "booz_nav.h"
#include "booz_estimator.h"
#include "booz_control.h"
#include "radio_control.h"
#ifndef DISABLE_NAV
float booz_nav_horizontal_x_sp;
float booz_nav_horizontal_y_sp;
float booz_nav_horizontal_pgain;
float booz_nav_horizontal_dgain;
float booz_nav_phi_command;
float booz_nav_theta_command;
#define BOOZ_NAV_HORIZONTAL_MAX_POS_ERR 10.
float booz_nav_vertical_z_sp;
float booz_nav_vertical_pgain;
float booz_nav_vertical_dgain;
#define BOOZ_NAV_VERTICAL_PGAIN 0.1
#define BOOZ_NAV_VERTICAL_DGAIN 0.1
#define BOOZ_NAV_VERT_HOVER_COMMAND 0.68
float booz_nav_power_command;
static void booz_nav_vertical_loop_run(void);
static void booz_nav_hovering_loop_run(void);
#endif
void booz_nav_init(void) {
#ifndef DISABLE_NAV
booz_nav_vertical_pgain = BOOZ_NAV_VERTICAL_PGAIN;
booz_nav_vertical_dgain = BOOZ_NAV_VERTICAL_DGAIN;
booz_nav_vertical_z_sp = -10.;
booz_nav_power_command = 0.; //BOOZ_NAV_VERT_HOVER_COMMAND;
#endif
}
void booz_nav_run(void) {
#ifndef DISABLE_NAV
booz_nav_vertical_loop_run();
booz_nav_hovering_loop_run();
BoozControlAttitudeSetSetPoints(booz_nav_phi_command, booz_nav_theta_command, 0., booz_nav_power_command);
booz_control_attitude_run();
#else
/* on real ac, let nav kill all motoros */
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
#endif
}
void booz_nav_read_setpoints_from_rc(void) {
#ifndef DISABLE_NAV
booz_nav_vertical_z_sp = -0 - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE];
booz_nav_horizontal_x_sp += 0.01 / MAX_PPRZ * (float)rc_values[RADIO_PITCH];
booz_nav_horizontal_y_sp += 0.01 / MAX_PPRZ * (float)rc_values[RADIO_ROLL];
#endif
}
#ifndef DISABLE_NAV
/* fisrt a vertical loop */
static void booz_nav_vertical_loop_run(void) {
float vertical_err = booz_estimator_z - booz_nav_vertical_z_sp;
Bound(vertical_err, -20., 20.);
booz_nav_power_command = BOOZ_NAV_VERT_HOVER_COMMAND +
booz_nav_vertical_pgain * vertical_err +
booz_nav_vertical_pgain * booz_estimator_vz ;
}
/* now a horizontal hovering loop */
static void booz_nav_hovering_loop_run(void) {
/* get a position error vector */
float x_error = booz_estimator_x - booz_nav_horizontal_x_sp;
float y_error = booz_estimator_y - booz_nav_horizontal_y_sp;
/* get norm and unit position error vector */
float norm_error = sqrt(x_error*x_error+ y_error*y_error);
float unit_x_error = x_error / norm_error;
float unit_y_error = y_error / norm_error;
Bound(norm_error, 0., BOOZ_NAV_HORIZONTAL_MAX_POS_ERR);
/* convert error vector to body frame */
float x_err_body = x_error * booz_estimator_dcm[AXIS_X][AXIS_X] +
y_error * booz_estimator_dcm[AXIS_X][AXIS_Y];
float y_err_body = x_error * booz_estimator_dcm[AXIS_Y][AXIS_X] +
y_error * booz_estimator_dcm[AXIS_Y][AXIS_Y];
booz_nav_phi_command = 0.;
booz_nav_theta_command = 0.;
}
#endif
sw/airborne/booz_nav.h
+24
View File
@@ -0,0 +1,24 @@
#ifndef BOOZ_NAV_H
#define BOOZ_NAV_H
#ifndef DISABLE_NAV
extern float booz_nav_horizontal_x_sp;
extern float booz_nav_horizontal_y_sp;
extern float booz_nav_horizontal_pgain;
extern float booz_nav_horizontal_dgain;
extern float booz_nav_phi_command;
extern float booz_nav_theta_command;
extern float booz_nav_vertical_z_sp;
extern float booz_nav_vertical_pgain;
extern float booz_nav_vertical_dgain;
extern float booz_nav_power_command;
#endif
extern void booz_nav_init(void);
extern void booz_nav_run(void);
extern void booz_nav_read_setpoints_from_rc(void);
#endif /* BOOZ_NAV_H */
sw/airborne/booz_telemetry.h
+26 -2
View File
@@ -12,6 +12,7 @@
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "booz_control.h"
#include "booz_nav.h"
#include "actuators_buss_twi_blmc_hw.h"
@@ -45,14 +46,37 @@
&booz_estimator_uf_r, &booz_control_r_sp );
#define PERIODIC_SEND_BOOZ_ATT_LOOP() \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, \
&booz_estimator_theta, &booz_control_theta_sp);
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
&booz_estimator_theta, &booz_control_attitude_theta_sp);
#define PERIODIC_SEND_BOOZ_UF_RATES() \
DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
&booz_estimator_uf_q, \
&booz_estimator_uf_r);
#ifndef DISABLE_NAV
#define PERIODIC_SEND_BOOZ_VERT_LOOP() { \
DOWNLINK_SEND_BOOZ_VERT_LOOP(&booz_nav_vertical_z_sp, \
&booz_estimator_vz, \
&booz_estimator_z, \
&booz_nav_power_command); \
}
#define PERIODIC_SEND_BOOZ_HOV_LOOP() { \
DOWNLINK_SEND_BOOZ_HOV_LOOP(&booz_nav_horizontal_x_sp, \
&booz_nav_horizontal_y_sp, \
&booz_estimator_vx, \
&booz_estimator_x, \
&booz_estimator_vy, \
&booz_estimator_y, \
&booz_nav_phi_command, \
&booz_nav_theta_command); \
}
#else
#define PERIODIC_SEND_BOOZ_VERT_LOOP() {}
#define PERIODIC_SEND_BOOZ_HOV_LOOP() {}
#endif
#define PERIODIC_SEND_BOOZ_CMDS() DOWNLINK_SEND_BOOZ_CMDS(&buss_twi_blmc_motor_power[SERVO_MOTOR_FRONT],\
&buss_twi_blmc_motor_power[SERVO_MOTOR_BACK], \
&buss_twi_blmc_motor_power[SERVO_MOTOR_LEFT], \
sw/simulator/Makefile
+1 -2
View File
@@ -141,13 +141,13 @@ CFLAGS += -DUSE_SPI
BOOZ_AB_SRCS += $(AB)/booz_link_mcu.c $(AB_ARCH)/booz_link_mcu_hw.c
BOOZ_AB_SRCS += $(AB)/spi.c $(AB_ARCH)/spi_hw.c
#../airborne/link_imu.c
#CFLAGS += -DDATALINK=PPRZ
#BOOZ_AB_SRCS += ../airborne/datalink.c
CFLAGS += -DBOOZ_FILTER_MCU
BOOZ_AB_SRCS += $(AB)/booz_estimator.c
BOOZ_AB_SRCS += $(AB)/booz_control.c
BOOZ_AB_SRCS += $(AB)/booz_nav.c
BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
BOOZ_AB_SRCS += $(AB)/commands.c
@@ -163,7 +163,6 @@ BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
BOOZ_AB_SRCS += $(AB)/multitilt.c
sw/simulator/booz_flightgear.c
+1 -1
View File
@@ -44,7 +44,7 @@ void booz_flightgear_send() {
gui.latitude = lat;
gui.longitude = lon;
gui.altitude = 30 - bfm.state->ve[BFMS_Z];
gui.altitude = 1.1 - bfm.state->ve[BFMS_Z];
gui.phi = bfm.state->ve[BFMS_PHI];
gui.theta = bfm.state->ve[BFMS_THETA];
sw/simulator/booz_sensors_model.c
+89 -26
View File
@@ -3,11 +3,35 @@
#include <math.h>
#include "booz_flight_model.h"
#include "booz_flight_model_utils.h"
struct BoozSensorsModel bsm;
static void booz_sensors_model_accel_init(void);
static void booz_sensors_model_accel_run(void);
static void booz_sensors_model_gyro_init(void);
static void booz_sensors_model_gyro_run(void);
static void booz_sensors_model_gps_init(void);
static void booz_sensors_model_gps_run(void);
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
void booz_sensors_model_init(void) {
booz_sensors_model_accel_init();
booz_sensors_model_gyro_init();
booz_sensors_model_gps_init();
}
void booz_sensors_model_run(void) {
booz_sensors_model_accel_run();
booz_sensors_model_gyro_run();
booz_sensors_model_gps_run();
}
static void booz_sensors_model_accel_init(void) {
bsm.accel = v_get(AXIS_NB);
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
@@ -15,7 +39,7 @@ void booz_sensors_model_init(void) {
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024 * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (1024. * 32) / (6. * 9.81);
@@ -25,15 +49,19 @@ void booz_sensors_model_init(void) {
bsm.accel_neutral->ve[AXIS_Z] = 506 * 32;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = 1e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_noise_std_dev->ve[AXIS_Y] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_noise_std_dev->ve[AXIS_Z] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = 1e-3 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_bias->ve[AXIS_Q] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_bias->ve[AXIS_R] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
}
static void booz_sensors_model_gyro_init(void) {
bsm.gyro = v_get(AXIS_NB);
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
@@ -51,26 +79,35 @@ void booz_sensors_model_init(void) {
bsm.gyro_neutral->ve[AXIS_R] = 39552;
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
bsm.gyro_noise_std_dev->ve[AXIS_P] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_noise_std_dev->ve[AXIS_P] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias = v_get(AXIS_NB);
bsm.gyro_bias->ve[AXIS_P] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias->ve[AXIS_Q] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias->ve[AXIS_R] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias->ve[AXIS_P] = 2e-2 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias->ve[AXIS_Q] = -2e-2 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias->ve[AXIS_R] = -1e-2 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
}
static void booz_sensors_model_gps_init(void) {
bsm.speed_sensor = v_get(AXIS_NB);
v_zero(bsm.speed_sensor);
bsm.pos_sensor = v_get(AXIS_NB);
v_zero(bsm.pos_sensor);
}
void booz_sensors_model_run(void) {
static void booz_sensors_model_accel_run(void) {
/* compute forces */
static VEC *accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
#if 0
accel_body = booz_flight_model_get_forces_body_frame(accel_body);
/* divide by mass */
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
// printf(" accel_body %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#if 0
/* get g in body frame */
/* extract eulers angles from state */
static VEC *eulers = VNULL;
@@ -97,15 +134,11 @@ void booz_sensors_model_run(void) {
bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel);
bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
/* gaussian noise */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_rand(accel_error);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
accel_error = v_mltadd(one, accel_error, -2., accel_error);
accel_error = v_star(accel_error, bsm.accel_noise_std_dev, accel_error);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* add per accel error reading */
@@ -116,7 +149,10 @@ void booz_sensors_model_run(void) {
bsm.accel->ve[AXIS_Z] = round( bsm.accel->ve[AXIS_Z]);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
}
static void booz_sensors_model_gyro_run(void) {
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
@@ -128,15 +164,16 @@ void booz_sensors_model_run(void) {
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* compute gyro error readinf*/
/* gaussian noise */
/* compute gyro error reading */
static VEC *gyro_error = VNULL;
gyro_error = v_resize(gyro_error, AXIS_NB);
gyro_error = v_rand(gyro_error);
gyro_error = v_mltadd(one, gyro_error, -2., gyro_error);
gyro_error = v_star(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
/* constant bias */
gyro_error = v_zero(gyro_error);
/* add a gaussian noise */
gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
/* add a constant bias */
gyro_error = v_add(bsm.gyro_bias, gyro_error, gyro_error);
/* add a random walk */
/* add per gyro error reading */
bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
/* round signal to account for adc discretisation */
@@ -145,3 +182,29 @@ void booz_sensors_model_run(void) {
bsm.gyro->ve[AXIS_R] = round( bsm.gyro->ve[AXIS_R]);
}
static void booz_sensors_model_gps_run(void) {
/* very wrong change me */
bsm.speed_sensor->ve[AXIS_X] = bfm.state->ve[BFMS_U];
bsm.speed_sensor->ve[AXIS_Y] = bfm.state->ve[BFMS_V];
bsm.speed_sensor->ve[AXIS_Z] = bfm.state->ve[BFMS_W];
bsm.pos_sensor->ve[AXIS_X] = bfm.state->ve[BFMS_X];
bsm.pos_sensor->ve[AXIS_Y] = bfm.state->ve[BFMS_Y];
bsm.pos_sensor->ve[AXIS_Z] = bfm.state->ve[BFMS_Z];
}
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = v_rand(tmp);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
tmp = v_mltadd(one, tmp, -2., tmp);
tmp = v_star(tmp, std_dev, tmp);
out = v_add(out, tmp, out);
return out;
}
sw/simulator/booz_sensors_model.h
+4
View File
@@ -21,6 +21,10 @@ struct BoozSensorsModel {
VEC* gyro_noise_std_dev;
VEC* gyro_bias;
/* imaginary sensors - gps maybe */
VEC* speed_sensor;
VEC* pos_sensor;
};
extern struct BoozSensorsModel bsm;
sw/simulator/main_booz_sim.c
+35 -13
View File
@@ -56,7 +56,10 @@ static gboolean booz_sim_periodic(gpointer data) {
/* read actuators positions */
booz_sim_read_actuators();
/* run our models */
if (sim_time > 3.)
/* no fdm at start to allow for filter initialisation */
/* it sucks, I know */
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run();
sim_time += DT;
@@ -70,16 +73,26 @@ static gboolean booz_sim_periodic(gpointer data) {
/* it will run the filter and the inter-process communication which */
/* will post a BoozLinkMcuEvent in the Controller process */
booz_filter_main_event_task();
/* process the BoozLinkMcuEvent */
/* this will update the controller estimator */
booz_controller_main_event_task();
/* in simulation feed speed and pos estimations ( with a pos sensor :( ) */
booz_estimator_set_speed_and_pos(bsm.speed_sensor->ve[AXIS_X],
bsm.speed_sensor->ve[AXIS_Y],
bsm.speed_sensor->ve[AXIS_Z],
bsm.pos_sensor->ve[AXIS_X],
bsm.pos_sensor->ve[AXIS_Y],
bsm.pos_sensor->ve[AXIS_Z] );
/* in simulation compute dcm as a helper for for nav */
booz_estimator_compute_dcm();
/* post a radio control event */
booz_sim_set_ppm_from_joystick();
ppm_valid = TRUE;
/* and let the controller process it */
booz_controller_main_event_task();
// printf("ppm_pulses %d\n", ppm_pulses[RADIO_THROTTLE]);
// printf("rc_value %d\n", rc_values[RADIO_THROTTLE]);
/* call the controller periodic task to run control loops */
booz_controller_main_periodic_task();
@@ -140,38 +153,47 @@ static inline void booz_sim_display(void) {
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) );
IvySendMsg("148 BOOZ_SIM_BODY_RATE %f %f %f",
IvySendMsg("148 BOOZ_SIM_BODY_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(bfm.state->ve[BFMS_P]),
DegOfRad(bfm.state->ve[BFMS_Q]),
DegOfRad(bfm.state->ve[BFMS_R]));
IvySendMsg("148 BOOZ_SIM_ATTITUDE %f %f %f",
DegOfRad(bfm.state->ve[BFMS_R]),
DegOfRad(bfm.state->ve[BFMS_PHI]),
DegOfRad(bfm.state->ve[BFMS_THETA]),
DegOfRad(bfm.state->ve[BFMS_PSI]));
IvySendMsg("148 BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
DegOfRad(bfm.state->ve[BFMS_X]),
DegOfRad(bfm.state->ve[BFMS_Y]),
DegOfRad(bfm.state->ve[BFMS_Z]),
DegOfRad(bfm.state->ve[BFMS_U]),
DegOfRad(bfm.state->ve[BFMS_V]),
DegOfRad(bfm.state->ve[BFMS_W]));
}
}
#define FAKE_JOYSTICK
static void booz_sim_set_ppm_from_joystick( void ) {
#ifndef FAKE_JOYSTICK
// printf("joystick_value %f\n", booz_joystick_value[JS_THROTTLE]);
ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
ppm_pulses[RADIO_PITCH] = 1498 + booz_joystick_value[JS_PITCH] * (2050-950);
ppm_pulses[RADIO_ROLL] = 1500 + booz_joystick_value[JS_ROLL] * (2050-950);
ppm_pulses[RADIO_YAW] = 1493 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950);
#else
int foo = sim_time/5;
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.7 * (2050-1223);
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 - 0.5 * (2050-950);
if (foo%2) {
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.5 * (2050-1223);
ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950);
}
else {
ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 - 0.2 * (2050-950);
}
else {
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.9 * (2050-1223);
ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950);
}
#endif
}