diff --git a/conf/autopilot/conf_booz.makefile b/conf/autopilot/conf_booz.makefile
new file mode 100644
index 0000000000..4f072c059f
--- /dev/null
+++ b/conf/autopilot/conf_booz.makefile
@@ -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
diff --git a/conf/messages.xml b/conf/messages.xml
index bd2c51e384..229e9cf5c0 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -429,22 +429,40 @@
-
-
-
-
-
-
+
+
+
+
+
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -452,35 +470,32 @@
-
+
-
+
-
-
-
-
+
+
+
+
-
+
-
-
-
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
index 2f1065529e..b649a3bda7 100644
--- a/conf/telemetry/booz.xml
+++ b/conf/telemetry/booz.xml
@@ -9,8 +9,10 @@
-
+
+
+
diff --git a/sw/airborne/booz_autopilot.c b/sw/airborne/booz_autopilot.c
index 7de62f6773..8398e36cf6 100644
--- a/sw/airborne/booz_autopilot.c
+++ b/sw/airborne/booz_autopilot.c
@@ -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;
}
-
}
diff --git a/sw/airborne/booz_autopilot.h b/sw/airborne/booz_autopilot.h
index ba49e1c034..887550fd21 100644
--- a/sw/airborne/booz_autopilot.h
+++ b/sw/airborne/booz_autopilot.h
@@ -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;
diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c
index eb2be97195..22e0aee8ca 100644
--- a/sw/airborne/booz_control.c
+++ b/sw/airborne/booz_control.c
@@ -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
-}
diff --git a/sw/airborne/booz_control.h b/sw/airborne/booz_control.h
index 1242919479..874bb3c322 100644
--- a/sw/airborne/booz_control.h
+++ b/sw/airborne/booz_control.h
@@ -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];
diff --git a/sw/airborne/booz_controller_main.c b/sw/airborne/booz_controller_main.c
index aa101bcaaa..743a290ccf 100644
--- a/sw/airborne/booz_controller_main.c
+++ b/sw/airborne/booz_controller_main.c
@@ -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
diff --git a/sw/airborne/booz_estimator.c b/sw/airborne/booz_estimator.c
index 11cf9dcaf2..c222584161 100644
--- a/sw/airborne/booz_estimator.c
+++ b/sw/airborne/booz_estimator.c
@@ -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 */
diff --git a/sw/airborne/booz_estimator.h b/sw/airborne/booz_estimator.h
index 6d541677e3..4f390e9847 100644
--- a/sw/airborne/booz_estimator.h
+++ b/sw/airborne/booz_estimator.h
@@ -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 */
diff --git a/sw/airborne/booz_nav.c b/sw/airborne/booz_nav.c
new file mode 100644
index 0000000000..9f3dc29854
--- /dev/null
+++ b/sw/airborne/booz_nav.c
@@ -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
diff --git a/sw/airborne/booz_nav.h b/sw/airborne/booz_nav.h
new file mode 100644
index 0000000000..d6f887055c
--- /dev/null
+++ b/sw/airborne/booz_nav.h
@@ -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 */
diff --git a/sw/airborne/booz_telemetry.h b/sw/airborne/booz_telemetry.h
index cde856f4fa..3f6cdcccbe 100644
--- a/sw/airborne/booz_telemetry.h
+++ b/sw/airborne/booz_telemetry.h
@@ -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], \
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 13460c4ba0..4ec5d65ae6 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -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
diff --git a/sw/simulator/booz_flightgear.c b/sw/simulator/booz_flightgear.c
index 58f474de88..53eab42a4f 100644
--- a/sw/simulator/booz_flightgear.c
+++ b/sw/simulator/booz_flightgear.c
@@ -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];
diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c
index 03958fcc46..d0b0ccabb5 100644
--- a/sw/simulator/booz_sensors_model.c
+++ b/sw/simulator/booz_sensors_model.c
@@ -3,11 +3,35 @@
#include
#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;
+}
diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h
index c608ced9ee..8e949c03df 100644
--- a/sw/simulator/booz_sensors_model.h
+++ b/sw/simulator/booz_sensors_model.h
@@ -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;
diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c
index a9dd0212b5..d64ae78fd0 100644
--- a/sw/simulator/main_booz_sim.c
+++ b/sw/simulator/main_booz_sim.c
@@ -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
}