diff --git a/Makefile b/Makefile
index 2c4a6a30be..1d60430fd7 100644
--- a/Makefile
+++ b/Makefile
@@ -119,7 +119,7 @@ bl:
cd $(AIRBORNE)/arm7/test/bootloader; make clean; make
upload_bl: bl
- lpc21isp -control $(AIRBORNE)/arm7/test/bootloader/bl.hex /dev/ttyS0 38400 12000
+ lpc21isp -control $(AIRBORNE)/arm7/test/bootloader/bl.hex /dev/ttyUSB0 38400 12000
lpc21iap:
cd sw/ground_segment/lpc21iap; make
diff --git a/conf/airframes/black_one.xml b/conf/airframes/black_one.xml
index 6a899b63e9..b0a2b116a9 100644
--- a/conf/airframes/black_one.xml
+++ b/conf/airframes/black_one.xml
@@ -97,6 +97,7 @@
+
diff --git a/conf/airframes/dragon.xml b/conf/airframes/dragon.xml
index 82760f5045..e72f24c460 100644
--- a/conf/airframes/dragon.xml
+++ b/conf/airframes/dragon.xml
@@ -40,17 +40,19 @@
+
+
+
diff --git a/conf/airframes/flyingtux.xml b/conf/airframes/flyingtux.xml
index f412ee898f..3e8aa1fb7e 100644
--- a/conf/airframes/flyingtux.xml
+++ b/conf/airframes/flyingtux.xml
@@ -61,14 +61,15 @@
+
+
@@ -101,7 +102,7 @@
-
+
diff --git a/conf/airframes/microjet1.xml b/conf/airframes/microjet1.xml
index 58b6a774a1..1d92a9fa33 100644
--- a/conf/airframes/microjet1.xml
+++ b/conf/airframes/microjet1.xml
@@ -56,6 +56,7 @@
+
+
+
-
diff --git a/conf/airframes/microjet4.xml b/conf/airframes/microjet4.xml
index 79703a45d4..72f0c38a98 100644
--- a/conf/airframes/microjet4.xml
+++ b/conf/airframes/microjet4.xml
@@ -99,6 +99,7 @@
+
diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml
index dfcf955270..0f0623ef2e 100644
--- a/conf/airframes/microjet5.xml
+++ b/conf/airframes/microjet5.xml
@@ -99,6 +99,7 @@
+
diff --git a/conf/airframes/microjet6.xml b/conf/airframes/microjet6.xml
index 39731889fc..4bcf1588df 100755
--- a/conf/airframes/microjet6.xml
+++ b/conf/airframes/microjet6.xml
@@ -90,6 +90,7 @@
+
@@ -98,7 +99,6 @@
-
@@ -137,12 +137,12 @@
+
+
+
+
+
+
-
-
+
+
+
+
+
@@ -32,6 +35,7 @@
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -132,8 +156,8 @@
@@ -146,13 +170,10 @@
-
-
include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
@@ -170,11 +191,11 @@ ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
-ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
+#TRANSPARENT ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
+#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
ap.CFLAGS += -DINTER_MCU
@@ -190,13 +211,15 @@ ap.srcs += gps_ubx.c gps.c
ap.CFLAGS += -DINFRARED
ap.srcs += infrared.c estimator.c
-ap.CFLAGS += -DNAV
-ap.srcs += nav.c pid.c
+ap.CFLAGS += -DNAV -DH_CTL_RATE_LOOP -DAGR_CLIMB
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
-ap.CFLAGS += -DGYRO -DADXRS150 -DPID_RATE_LOOP
+ap.CFLAGS += -DGYRO -DADXRS150
ap.srcs += gyro.c
+ap.srcs += bomb.c
+
# Harware In The Loop
#ap.CFLAGS += -DHITL
@@ -204,7 +227,8 @@ ap.srcs += gyro.c
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DCONFIG=\"tiny.h\"
+sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB
+sim.srcs += bomb.c
# a test program to setup actuators
@@ -213,7 +237,7 @@ setup_actuators.ARCH = arm7tdmi
setup_actuators.TARGET = setup_actuators
setup_actuators.TARGETDIR = setup_actuators
-setup_actuators.CFLAGS += -DFBW -DCONFIG=\"tiny_1_1.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
+setup_actuators.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
@@ -223,15 +247,7 @@ tunnel.ARCH = arm7tdmi
tunnel.TARGET = tunnel
tunnel.TARGETDIR = tunnel
-tunnel.CFLAGS += -DFBW -DCONFIG=\"tiny_1_1.h\" -DLED -DTIME_LED=1
+tunnel.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1
tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
-
-test_led.ARCHDIR = $(ARCHI)
-test_led.ARCH = arm7tdmi
-test_led.TARGET = tunnel
-test_led.TARGETDIR = tunnel
-test_led.CFLAGS += -DFBW -DCONFIG=\"tiny_1_1.h\" -DLED
-test_led.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c test_led.c main.c
-
diff --git a/conf/airframes/plaster1.xml b/conf/airframes/plaster1.xml
index 209696c828..d831b64dea 100755
--- a/conf/airframes/plaster1.xml
+++ b/conf/airframes/plaster1.xml
@@ -62,7 +62,8 @@
-
+
+
+
@@ -110,7 +112,7 @@
-
+
diff --git a/conf/airframes/slayer1.xml b/conf/airframes/slayer1.xml
index fe39a05e36..5e5be51095 100644
--- a/conf/airframes/slayer1.xml
+++ b/conf/airframes/slayer1.xml
@@ -116,18 +116,18 @@
-
+
+
+
+
+
+
+
diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml
index 386777faf4..b0f5119fef 100644
--- a/conf/airframes/twinjet1.xml
+++ b/conf/airframes/twinjet1.xml
@@ -80,6 +80,7 @@
+
@@ -97,7 +98,6 @@
-
@@ -130,7 +130,7 @@
-
+
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index 40bce1a27a..f9320153a4 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -57,6 +57,7 @@
+
+
+
+
-
+
diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml
index 848e6f497c..b38c6db433 100644
--- a/conf/airframes/twinstar2.xml
+++ b/conf/airframes/twinstar2.xml
@@ -60,6 +60,7 @@
+
+
+
-
+
diff --git a/conf/airframes/twinstar4.xml b/conf/airframes/twinstar4.xml
index 73d90c2644..659c1f8815 100755
--- a/conf/airframes/twinstar4.xml
+++ b/conf/airframes/twinstar4.xml
@@ -64,6 +64,7 @@
+
+
@@ -106,7 +108,7 @@
-
+
diff --git a/conf/airframes/twinstar5.xml b/conf/airframes/twinstar5.xml
index 0cddf39def..53ebda927d 100644
--- a/conf/airframes/twinstar5.xml
+++ b/conf/airframes/twinstar5.xml
@@ -64,6 +64,7 @@
+
@@ -104,7 +104,7 @@
-
+
diff --git a/conf/airframes/twinstar6.xml b/conf/airframes/twinstar6.xml
index b1d4ac995b..14e113e745 100644
--- a/conf/airframes/twinstar6.xml
+++ b/conf/airframes/twinstar6.xml
@@ -70,6 +70,7 @@
+
@@ -110,7 +110,7 @@
-
+
diff --git a/conf/airframes/twinstarmm.xml b/conf/airframes/twinstarmm.xml
index d44b5b3451..2accf73d86 100644
--- a/conf/airframes/twinstarmm.xml
+++ b/conf/airframes/twinstarmm.xml
@@ -80,18 +80,15 @@
+
-
-
-
-
@@ -131,7 +128,7 @@
-
+
diff --git a/conf/autopilot/sitl.makefile b/conf/autopilot/sitl.makefile
index f1e04ee23b..59f815bc81 100644
--- a/conf/autopilot/sitl.makefile
+++ b/conf/autopilot/sitl.makefile
@@ -3,4 +3,4 @@ sim.ARCH = sitl
sim.TARGET = autopilot
sim.TARGETDIR = autopilot
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DRADIO_CONTROL_SETTINGS
-sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c pid.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c
+sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c
diff --git a/conf/messages.xml b/conf/messages.xml
index cd936df525..7f83e6d5fa 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -68,7 +68,7 @@
-
+
@@ -465,7 +465,7 @@
-
+
diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index 4b709415de..f160f245a4 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -18,20 +18,20 @@
-
-
-
-
+
+
+
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index c6e3c51ea6..bb563c3c0c 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -54,7 +54,7 @@
#define PERIODIC_SEND_IDENT() DOWNLINK_SEND_IDENT(&ac_ident);
-#define PERIODIC_SEND_BAT() Downlink({ int16_t e = energy; DOWNLINK_SEND_BAT(&desired_gaz, &vsupply, &estimator_flight_time, &kill_throttle, &block_time, &stage_time, &e); })
+#define PERIODIC_SEND_BAT() Downlink({ int16_t e = energy; DOWNLINK_SEND_BAT(&v_ctl_throttle_setpoint, &vsupply, &estimator_flight_time, &kill_throttle, &block_time, &stage_time, &e); })
#ifdef MCU_SPI_LINK
#define PERIODIC_SEND_DEBUG_MCU_LINK() DOWNLINK_SEND_DEBUG_MCU_LINK(&link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
@@ -79,7 +79,7 @@
})
#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status, &ir_estim_mode);
-#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude, &desired_climb);
+#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&h_ctl_roll_setpoint, &h_ctl_pitch_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
@@ -119,14 +119,18 @@
#ifdef IDG300
#include "gyro.h"
-#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate_adc, &roll_rate, &pitch_rate)
+#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate_adc, &estimator_p, &estimator_q)
#elif defined ADXRS150
-#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate_adc, &roll_rate, &temp_comp)
+#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate_adc, &estimator_p, &temp_comp)
#else
#define PERIODIC_SEND_GYRO_RATES() {}
#endif
-#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain, &climb_gaz_submode)
+#ifdef AGR_CLIMB
+#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_pgain, &h_ctl_course_pgain, &v_ctl_auto_throttle_submode)
+#else
+#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_pgain, &h_ctl_course_pgain, &pprz_mode)
+#endif
#define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); }
@@ -138,7 +142,7 @@
#define PERIODIC_SEND_IMU() {}
#endif
-#define SEND_NAVIGATION() Downlink({ int16_t pos_x = estimator_x; int16_t pos_y = estimator_y; int16_t d_course = DeciDegOfRad(desired_course); DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &pos_x, &pos_y, &d_course, &dist2_to_wp, &dist2_to_home);})
+#define SEND_NAVIGATION() Downlink({ int16_t pos_x = estimator_x; int16_t pos_y = estimator_y; int16_t d_course = DeciDegOfRad(h_ctl_course_pgain); DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &pos_x, &pos_y, &d_course, &dist2_to_wp, &dist2_to_home);})
#ifdef CAM
#define SEND_CAM() Downlink({ int16_t x = target_x; int16_t y = target_y; int8_t phi = DegOfRad(phi_c); int8_t theta = DegOfRad(theta_c); DOWNLINK_SEND_CAM(&phi, &theta, &x, &y);})
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index 6e500ac8b6..eefa14bddb 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -38,7 +38,8 @@
#include "messages.h"
#include "estimator.h"
-#include "pid.h"
+#include "fw_v_ctl.h"
+#include "fw_h_ctl.h"
#include "cam.h"
#include "infrared.h"
#include "gps.h"
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index d8ccb0e815..f5dbf596e6 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/estimator.c
@@ -44,6 +44,10 @@ float estimator_phi;
float estimator_psi;
float estimator_theta;
+/* rates in radians per second */
+float estimator_p;
+float estimator_q;
+
/* flight time in seconds */
uint16_t estimator_flight_time;
/* flight time in seconds */
@@ -86,6 +90,8 @@ void estimator_init( void ) {
EstimatorSetSpeedPol ( 0., 0., 0.);
+ EstimatorSetRate(0., 0.);
+
estimator_flight_time = 0;
}
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
index 6d6a2b1616..e5ca069d38 100644
--- a/sw/airborne/estimator.h
+++ b/sw/airborne/estimator.h
@@ -47,6 +47,10 @@ extern float estimator_theta;
/* speed in meters per second */
extern float estimator_z_dot;
+/* rates in radians per second */
+extern float estimator_p;
+extern float estimator_q;
+
/* flight time in seconds */
extern uint16_t estimator_flight_time;
extern float estimator_t;
@@ -78,5 +82,6 @@ void estimator_propagate_state( void );
#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
#define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
+#define EstimatorSetRate(p, q) { estimator_p = p; estimator_q = q; }
#endif /* ESTIMATOR_H */
diff --git a/sw/airborne/fw_h_ctl.c b/sw/airborne/fw_h_ctl.c
new file mode 100644
index 0000000000..8a7f9e5080
--- /dev/null
+++ b/sw/airborne/fw_h_ctl.c
@@ -0,0 +1,208 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
+ *
+ * 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.
+ *
+ */
+
+/**
+ *
+ * fixed wing horizontal control
+ *
+ */
+
+#include "fw_h_ctl.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+#include "fw_v_ctl.h"
+
+
+/* outer loop parameters */
+float h_ctl_course_setpoint;
+float h_ctl_course_pre_bank;
+float h_ctl_course_pgain;
+float h_ctl_roll_max_setpoint;
+
+/* inner roll loop parameters */
+float h_ctl_roll_setpoint;
+float h_ctl_roll_pgain;
+pprz_t h_ctl_aileron_setpoint;
+
+/* inner pitch loop parameters */
+float h_ctl_pitch_setpoint;
+float h_ctl_pitch_pgain;
+pprz_t h_ctl_elevator_setpoint;
+
+/* inner loop pre-command */
+float h_ctl_aileron_of_throttle;
+float h_ctl_elevator_of_roll;
+
+/* rate loop */
+#ifdef H_CTL_RATE_LOOP
+float h_ctl_roll_rate_mode;
+float h_ctl_roll_rate_setpoint_pgain;
+float h_ctl_roll_rate_pgain;
+float h_ctl_roll_rate_igain;
+float h_ctl_roll_rate_dgain;
+#endif
+
+inline static void h_ctl_roll_loop( void );
+inline static void h_ctl_pitch_loop( void );
+#ifdef H_CTL_RATE_LOOP
+static inline void h_ctl_roll_rate_loop( float err_roll );
+#endif
+
+void h_ctl_init( void ) {
+
+ h_ctl_course_setpoint = 0.;
+ h_ctl_course_pre_bank = 0.;
+ h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
+ h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
+
+ h_ctl_roll_setpoint = 0.;
+ h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
+ h_ctl_aileron_setpoint = 0;
+ h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
+
+ h_ctl_pitch_setpoint = 0.;
+ h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
+ h_ctl_elevator_setpoint = 0;
+ h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
+
+#ifdef H_CTL_RATE_LOOP
+ h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
+ h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
+ h_ctl_roll_rate_pgain = H_CTL_ROLL_RATE_PGAIN;
+ h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
+ h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
+#endif
+}
+
+/**
+ * \brief
+ *
+ */
+void h_ctl_course_loop ( void ) {
+ float err = estimator_hspeed_dir - h_ctl_course_setpoint;
+ NormRadAngle(err);
+ float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+ Bound(speed_depend_nav, 0.66, 1.5);
+ float cmd = h_ctl_course_pgain * err * speed_depend_nav;
+#if defined AGR_CLIMB
+ if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE) {
+ float altitude_error = estimator_z - v_ctl_altitude_setpoint;
+ cmd *= ((altitude_error < 0) ? AGR_CLIMB_NAV_RATIO : AGR_DESCENT_NAV_RATIO);
+ }
+#endif
+ h_ctl_roll_setpoint = cmd + h_ctl_course_pre_bank;
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
+}
+
+void h_ctl_attitude_loop ( void ) {
+ h_ctl_roll_loop();
+ h_ctl_pitch_loop();
+}
+
+inline static void h_ctl_roll_loop( void ) {
+ float err = estimator_phi - h_ctl_roll_setpoint;
+ float cmd = h_ctl_roll_pgain * err
+ + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
+ h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
+#ifdef H_CTL_RATE_LOOP
+ h_ctl_roll_rate_loop(err);
+#endif
+}
+
+#ifdef H_CTL_RATE_LOOP
+static inline void h_ctl_roll_rate_loop( float err_roll ) {
+ float roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err_roll;
+ BoundAbs(roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
+
+ float err = estimator_p - roll_rate_setpoint;
+
+ /* I term calculation */
+ static float roll_rate_sum_err = 0.;
+ static uint8_t roll_rate_sum_idx = 0;
+ static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
+
+ roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
+ roll_rate_sum_values[roll_rate_sum_idx] = err;
+ roll_rate_sum_err += err;
+ roll_rate_sum_idx++;
+ if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0;
+
+ /* D term calculations */
+ static float last_err = 0;
+ float d_err = err - last_err;
+ last_err = err;
+
+ float cmd = h_ctl_roll_rate_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err);
+ cmd = TRIM_PPRZ(cmd);
+
+ h_ctl_aileron_setpoint = h_ctl_roll_rate_mode * cmd
+ + (1 - h_ctl_roll_rate_mode) * h_ctl_aileron_setpoint;
+}
+#endif /* H_CTL_RATE_LOOP */
+
+inline static void h_ctl_pitch_loop( void ) {
+ /* sanity check */
+ if (h_ctl_elevator_of_roll <0.)
+ h_ctl_elevator_of_roll = 0.;
+ float err = estimator_theta - h_ctl_pitch_setpoint;
+ float cmd = err * h_ctl_pitch_pgain
+ + h_ctl_elevator_of_roll * fabs(estimator_phi);
+#ifdef LOITER_TRIM
+ cmd += loiter();
+#endif
+ h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
+}
+
+
+
+
+
+#ifdef LOITER_TRIM
+
+float loiter_trim = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
+float dash_trim = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
+
+inline static void loiter(void) {
+ static float last_elevator_trim = 0;
+ float elevator_trim;
+ Bound(v_ctl_auto_throttle_cruise_throttle,
+ V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE,
+ V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+ float cruise_trim = v_ctl_auto_throttle_cruise_throttle
+ - V_CTL_AUTO_THROTTLE_CRUISE_THROTTLE;
+ float max_change = (loiter_trim-dash_trim)/80.;
+ if (cruise_trim > 0) {
+ elevator_trim = cruise_trim * dash_trim / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - V_CTL_AUTO_THROTTLE_CRUISE_THROTTLE);
+ elevator_trim=Max(last_elevator_trim - max_change, elevator_trim);
+ }
+ else {
+ elevator_trim = cruise_trim * loiter_trim / (V_CTL_AUTO_THROTTLE_CRUISE_THROTTLE- V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE);
+ elevator_trim=Min(last_elevator_trim+ max_change, elevator_trim);
+ }
+ last_elevator_trim = elevator_trim;
+ return elevator_trim;
+}
+#endif
+
diff --git a/sw/airborne/fw_h_ctl.h b/sw/airborne/fw_h_ctl.h
new file mode 100644
index 0000000000..0b992bc4e2
--- /dev/null
+++ b/sw/airborne/fw_h_ctl.h
@@ -0,0 +1,70 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
+ *
+ * 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.
+ *
+ */
+
+/**
+ *
+ * fixed wing horizontal control
+ *
+ */
+
+#ifndef FW_H_CTL_H
+#define FW_H_CTL_H
+
+#include
+#include "paparazzi.h"
+
+/* outer loop parameters */
+extern float h_ctl_course_setpoint;
+extern float h_ctl_course_pre_bank;
+extern float h_ctl_course_pgain;
+extern float h_ctl_roll_max_setpoint;
+
+/* inner roll loop parameters */
+extern float h_ctl_roll_setpoint;
+extern float h_ctl_roll_pgain;
+extern pprz_t h_ctl_aileron_setpoint;
+
+/* inner pitch loop parameters */
+extern float h_ctl_pitch_setpoint;
+extern float h_ctl_pitch_pgain;
+extern pprz_t h_ctl_elevator_setpoint;
+
+/* inner loop pre-command */
+extern float h_ctl_aileron_of_throttle;
+extern float h_ctl_elevator_of_roll;
+
+/* rate loop */
+#ifdef H_CTL_RATE_LOOP
+extern float h_ctl_roll_rate_mode;
+extern float h_ctl_roll_rate_setpoint_pgain;
+extern float h_ctl_roll_rate_pgain;
+extern float h_ctl_roll_rate_igain;
+extern float h_ctl_roll_rate_dgain;
+#endif
+
+extern void h_ctl_init( void );
+extern void h_ctl_course_loop ( void );
+extern void h_ctl_attitude_loop ( void );
+
+#endif /* FW_H_CTL_H */
diff --git a/sw/airborne/fw_v_ctl.c b/sw/airborne/fw_v_ctl.c
new file mode 100644
index 0000000000..b39691f786
--- /dev/null
+++ b/sw/airborne/fw_v_ctl.c
@@ -0,0 +1,223 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
+ *
+ * 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.
+ *
+ */
+
+/**
+ * \file v_ctl_ctl
+ * \brief Vertical control for fixed wing vehicles.
+ *
+ */
+
+#include "fw_v_ctl.h"
+//#include "autopilot.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+
+/* outer loop */
+float v_ctl_altitude_setpoint;
+float v_ctl_altitude_pre_climb;
+float v_ctl_altitude_pgain;
+float v_ctl_altitude_error;
+
+/* inner loop */
+float v_ctl_climb_setpoint;
+uint8_t v_ctl_climb_mode;
+uint8_t v_ctl_auto_throttle_submode;
+
+/* "auto throttle" inner loop parameters */
+float v_ctl_auto_throttle_cruise_throttle;
+float v_ctl_auto_throttle_climb_throttle_increment;
+float v_ctl_auto_throttle_pgain;
+float v_ctl_auto_throttle_igain;
+float v_ctl_auto_throttle_sum_err;
+#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
+float v_ctl_auto_throttle_pitch_of_vz_pgain;
+
+/* "auto pitch" inner loop parameters */
+float v_ctl_auto_pitch_pgain;
+float v_ctl_auto_pitch_igain;
+float v_ctl_auto_pitch_sum_err;
+#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
+
+pprz_t v_ctl_throttle_setpoint;
+
+inline static void v_ctl_climb_auto_throttle_loop( void );
+inline static void v_ctl_climb_auto_pitch_loop( void );
+
+void v_ctl_init( void ) {
+ /* outer loop */
+ v_ctl_altitude_setpoint = 0.;
+ v_ctl_altitude_pre_climb = 0.;
+ v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
+ v_ctl_altitude_error = 0.;
+
+ /* inner loops */
+ v_ctl_climb_setpoint = 0.;
+ v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
+#ifdef AGR_CLIMB
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
+#endif
+
+ /* "auto throttle" inner loop parameters */
+ v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_CRUISE_THROTTLE;
+ v_ctl_auto_throttle_climb_throttle_increment =
+ V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
+ v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
+ v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
+ v_ctl_auto_throttle_sum_err = 0.;
+ v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
+
+ /* "auto pitch" inner loop parameters */
+ v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
+ v_ctl_auto_pitch_igain = V_CTL_AUTO_PITCH_IGAIN;
+ v_ctl_auto_pitch_sum_err = 0.;
+
+ v_ctl_throttle_setpoint = 0;
+}
+
+/**
+ * outer loop
+ * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
+ */
+void v_ctl_altitude_loop( void ) {
+ //mmmm LOOKATME : should that be in nav ???
+ v_ctl_altitude_setpoint = nav_altitude + altitude_shift;
+
+ v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
+ v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error
+ + v_ctl_altitude_pre_climb;
+ BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
+
+#ifdef AGR_CLIMB
+ if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
+ float dist = fabs(v_ctl_altitude_error);
+ if (dist < AGR_BLEND_END) {
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
+ }
+ else if (dist > AGR_BLEND_START) {
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_AGRESSIVE;
+ }
+ else {
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_BLENDED;
+ }
+ }
+#endif
+}
+
+void v_ctl_climb_loop ( void ) {
+ switch (v_ctl_climb_mode) {
+ case V_CTL_CLIMB_MODE_AUTO_THROTTLE:
+ v_ctl_climb_auto_throttle_loop();
+ break;
+ case V_CTL_CLIMB_MODE_AUTO_PITCH:
+ v_ctl_climb_auto_pitch_loop();
+ break;
+ }
+}
+
+/**
+ * auto throttle inner loop
+ * \brief
+ */
+inline static void v_ctl_climb_auto_throttle_loop(void) {
+ float f_throttle = 0;
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ + v_ctl_auto_throttle_pgain *
+ (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err);
+
+ /* pitch pre-command */
+ float v_ctl_pitch_of_vz = v_ctl_climb_setpoint * v_ctl_auto_throttle_pitch_of_vz_pgain;
+
+#if defined AGR_CLIMB
+ switch (v_ctl_auto_throttle_submode) {
+ case V_CTL_AUTO_THROTTLE_AGRESSIVE:
+ if (v_ctl_climb_setpoint > 0) { /* Climbing */
+ f_throttle = AGR_CLIMB_THROTTLE;
+ nav_pitch = AGR_CLIMB_PITCH;
+ }
+ else { /* Going down */
+ f_throttle = AGR_DESCENT_THROTTLE;
+ nav_pitch = AGR_DESCENT_PITCH;
+ }
+ break;
+
+ case V_CTL_AUTO_THROTTLE_BLENDED: {
+ float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)
+ / (AGR_BLEND_START - AGR_BLEND_END);
+ f_throttle = (1-ratio) * controlled_throttle;
+ nav_pitch = (1-ratio) * v_ctl_pitch_of_vz;
+ v_ctl_auto_throttle_sum_err += (1-ratio) * err;
+ if (v_ctl_altitude_error < 0) {
+ f_throttle += ratio * AGR_CLIMB_THROTTLE;
+ nav_pitch += ratio * AGR_CLIMB_PITCH;
+ } else {
+ f_throttle += ratio * AGR_DESCENT_THROTTLE;
+ nav_pitch += ratio * AGR_DESCENT_PITCH;
+ }
+ break;
+ }
+
+ case V_CTL_AUTO_THROTTLE_STANDARD:
+#endif
+ f_throttle = controlled_throttle;
+ v_ctl_auto_throttle_sum_err += err;
+ BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
+ nav_pitch += v_ctl_pitch_of_vz;
+#if defined AGR_CLIMB
+ break;
+ } /* switch submode */
+#endif
+ v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
+}
+
+
+/**
+ * auto pitch inner loop
+ * \brief computes a nav_pitch from a climb_setpoint given a fixed throttle
+ */
+inline static void v_ctl_climb_auto_pitch_loop(void) {
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ v_ctl_throttle_setpoint = nav_desired_gaz;
+ v_ctl_auto_pitch_sum_err += err;
+ BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
+ nav_pitch = v_ctl_auto_pitch_pgain *
+ (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
+ Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
+}
+
+#ifndef V_CTL_THROTTLE_SLEW
+#define V_CTL_THROTTLE_SLEW 1.
+#endif
+void v_ctl_throttle_slew( void ) {
+ static pprz_t last_throttle;
+ pprz_t diff_throttle = v_ctl_throttle_setpoint - last_throttle;
+ BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
+ v_ctl_throttle_setpoint = last_throttle + diff_throttle;
+ last_throttle = v_ctl_throttle_setpoint;
+}
+
+
+
diff --git a/sw/airborne/fw_v_ctl.h b/sw/airborne/fw_v_ctl.h
new file mode 100644
index 0000000000..04af6ed841
--- /dev/null
+++ b/sw/airborne/fw_v_ctl.h
@@ -0,0 +1,75 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2006 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ *
+ * fixed wing vertical control
+ *
+ */
+
+#ifndef FW_V_CTL_H
+#define FW_V_CTL_H
+
+#include
+#include "paparazzi.h"
+
+/* outer loop */
+extern float v_ctl_altitude_setpoint;
+extern float v_ctl_altitude_pre_climb;
+extern float v_ctl_altitude_pgain;
+
+/* inner loop */
+extern float v_ctl_climb_setpoint;
+extern uint8_t v_ctl_climb_mode;
+#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0
+#define V_CTL_CLIMB_MODE_AUTO_PITCH 1
+
+#ifdef AGR_CLIMB
+extern uint8_t v_ctl_auto_throttle_submode;
+#define V_CTL_AUTO_THROTTLE_STANDARD 0
+#define V_CTL_AUTO_THROTTLE_AGRESSIVE 1
+#define V_CTL_AUTO_THROTTLE_BLENDED 2
+#endif
+
+/* "auto throttle" inner loop parameters */
+extern float v_ctl_auto_throttle_cruise_throttle;
+extern float v_ctl_auto_throttle_climb_throttle_increment;
+extern float v_ctl_auto_throttle_pgain;
+extern float v_ctl_auto_throttle_igain;
+extern float v_ctl_auto_throttle_sum_err;
+extern float v_ctl_auto_throttle_pitch_of_vz_pgain;
+
+/* "auto pitch" inner loop parameters */
+extern float v_ctl_auto_pitch_pgain;
+extern float v_ctl_auto_pitch_igain;
+extern float v_ctl_auto_pitch_sum_err;
+
+extern pprz_t v_ctl_throttle_setpoint;
+
+extern void v_ctl_init( void );
+extern void v_ctl_altitude_loop( void );
+extern void v_ctl_climb_loop ( void );
+extern void v_ctl_throttle_slew( void );
+
+#endif /* FW_V_CTL_H */
diff --git a/sw/airborne/gyro.c b/sw/airborne/gyro.c
index 917007a1b8..b2a9ef2c37 100644
--- a/sw/airborne/gyro.c
+++ b/sw/airborne/gyro.c
@@ -32,10 +32,9 @@
#include "std.h"
#include "adc.h"
#include "airframe.h"
+#include "estimator.h"
int16_t roll_rate_adc;
-float roll_rate;
-
static struct adc_buf buf_roll;
#define RadiansOfADC(_adc, scale) RadOfDeg((_adc * scale))
@@ -44,13 +43,12 @@ static struct adc_buf buf_roll;
static struct adc_buf buf_temp;
float temp_comp;
#elif defined IDG300
+int16_t pitch_rate_adc;
static struct adc_buf buf_pitch;
-float pitch_rate;
#endif
void gyro_init( void) {
adc_buf_channel(ADC_CHANNEL_GYRO_ROLL, &buf_roll, ADC_CHANNEL_GYRO_NB_SAMPLES);
-
#if defined ADXRS150
adc_buf_channel(ADC_CHANNEL_GYRO_TEMP, &buf_temp, ADC_CHANNEL_GYRO_NB_SAMPLES);
#elif defined IDG300
@@ -61,13 +59,15 @@ void gyro_init( void) {
void gyro_update( void ) {
+ float pitch_rate = 0.;
roll_rate_adc = (buf_roll.sum/buf_roll.av_nb_sample) - GYRO_ADC_ROLL_NEUTRAL;
#ifdef ADXRS150
temp_comp = buf_temp.sum/buf_temp.av_nb_sample - GYRO_ADC_TEMP_NEUTRAL;
roll_rate_adc += GYRO_ADC_TEMP_SLOPE * temp_comp;
#elif defined IDG300
- pitch_rate = buf_pitch.sum/buf_pitch.av_nb_sample - GYRO_ADC_PITCH_NEUTRAL;
- pitch_rate = GYRO_PITCH_DIRECTION * RadiansOfADC(pitch_rate, GYRO_PITCH_SCALE);
+ pitch_rate_adc = buf_pitch.sum/buf_pitch.av_nb_sample - GYRO_ADC_PITCH_NEUTRAL;
+ pitch_rate = GYRO_PITCH_DIRECTION * RadiansOfADC(pitch_rate_adc, GYRO_PITCH_SCALE);
#endif
- roll_rate = GYRO_ROLL_DIRECTION * RadiansOfADC(roll_rate_adc, GYRO_ROLL_SCALE);
+ float roll_rate = GYRO_ROLL_DIRECTION * RadiansOfADC(roll_rate_adc, GYRO_ROLL_SCALE);
+ EstimatorSetRate(roll_rate, pitch_rate);
}
diff --git a/sw/airborne/gyro.h b/sw/airborne/gyro.h
index b550928ca7..d1273a25c7 100644
--- a/sw/airborne/gyro.h
+++ b/sw/airborne/gyro.h
@@ -32,9 +32,6 @@
#include
-/** In rad/s */
-extern float roll_rate;
-
/** Raw (for debug), taking into accound neutral and temp compensation (if any) */
extern int16_t roll_rate_adc;
@@ -43,7 +40,7 @@ extern int16_t roll_rate_adc;
#if defined ADXRS150
extern float temp_comp;
#elif defined IDG300
-extern float pitch_rate;
+extern int16_t pitch_rate_adc;
#endif
void gyro_init( void );
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index dce5ea5991..900fe224d1 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -36,7 +36,8 @@
#include "interrupt_hw.h"
#include "init_hw.h"
#include "adc.h"
-#include "pid.h"
+#include "fw_h_ctl.h"
+#include "fw_v_ctl.h"
#include "gps.h"
#include "infrared.h"
#include "gyro.h"
@@ -249,15 +250,15 @@ inline void telecommand_task( void ) {
*/
if (pprz_mode == PPRZ_MODE_AUTO1) {
/** In Auto1 mode, roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
- desired_roll = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL);
+ h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL);
/** In Auto1 mode, pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
- desired_pitch = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
+ h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
}
if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
- desired_gaz = fbw_state->channels[RADIO_THROTTLE];
+ v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
}
- /** else asynchronously set by climb_pid_run(); */
+ /** else asynchronously set by v_ctl_climb_loop(); */
mcu1_ppm_cpt = fbw_state->ppm_cpt;
vsupply = fbw_state->vsupply;
@@ -319,22 +320,21 @@ static void navigation_task( void ) {
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (vertical_mode == VERTICAL_MODE_AUTO_ALT)
- altitude_pid_run();
+ v_ctl_altitude_loop();
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
if (lateral_mode >=LATERAL_MODE_COURSE)
- course_pid_run(); /* aka compute nav_desired_roll */
- desired_roll = nav_desired_roll;
+ h_ctl_course_loop(); /* aka compute nav_desired_roll */
if (vertical_mode >= VERTICAL_MODE_AUTO_CLIMB)
- climb_pid_run();
+ v_ctl_climb_loop();
if (vertical_mode == VERTICAL_MODE_AUTO_GAZ)
- desired_gaz = nav_desired_gaz;
- desired_pitch = nav_pitch;
+ v_ctl_throttle_setpoint = nav_desired_gaz;
+ h_ctl_pitch_setpoint = nav_pitch;
if (kill_throttle || (!estimator_flight_time && !launch))
- desired_gaz = 0;
+ v_ctl_throttle_setpoint = 0;
}
- energy += (float)desired_gaz * (MILLIAMP_PER_PERCENT / MAX_PPRZ * 0.25);
+ energy += (float)v_ctl_throttle_setpoint * (MILLIAMP_PER_PERCENT / MAX_PPRZ * 0.25);
}
@@ -354,9 +354,9 @@ static void navigation_task( void ) {
* - lets use \a reporting_task at 10 Hz
* - updates ir with \a ir_update
* - updates estimator of ir with \a estimator_update_state_infrared
- * - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop_run
- * - sends to \a fbw \a desired_gaz, \a desired_aileron and
- * \a desired_elevator \note \a desired_gaz is set upon GPS
+ * - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
+ * - sends to \a fbw \a desired_throttle, \a desired_aileron and
+ * \a desired_elevator \note \a desired_throttle is set upon GPS
* message reception
* - 10 Hz: to get a \a stage_time_ds
* - 4 Hz:
@@ -461,11 +461,11 @@ void periodic_task_ap( void ) {
ir_update();
estimator_update_state_infrared();
#endif /* INFRARED */
- pid_attitude_loop_run(); /* Set desired_aileron & desired_elevator */
- pid_slew_gaz();
- ap_state->commands[COMMAND_THROTTLE] = desired_gaz;
- ap_state->commands[COMMAND_ROLL] = desired_aileron;
- ap_state->commands[COMMAND_PITCH] = desired_elevator;
+ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
+ v_ctl_throttle_slew();
+ ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_setpoint;
+ ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
+ ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#ifdef COMMAND_HATCH_CMD
extern pprz_t hatch_cmd;
@@ -542,7 +542,8 @@ void init_ap( void ) {
#endif
/************ Internal status ***************/
- pid_init();
+ h_ctl_init();
+ v_ctl_init();
estimator_init();
nav_init();
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index eeff171f10..9ddfe5a802 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -33,7 +33,8 @@
#include "nav.h"
#include "gps.h"
#include "estimator.h"
-#include "pid.h"
+#include "fw_h_ctl.h"
+#include "fw_v_ctl.h"
#include "autopilot.h"
#include "inter_mcu.h"
#include "airframe.h"
@@ -306,18 +307,6 @@ static unit_t reset_nav_reference( void ) __attribute__ ((unused));
static unit_t reset_waypoints( void ) __attribute__ ((unused));
-extern float nav_altitude;
-
-#define CLIMB_MODE_GAZ 0
-#define CLIMB_MODE_PITCH 1
-
-#define CLIMB_MODE_GAZ_STANDARD 0
-#define CLIMB_MODE_GAZ_AGRESSIVE 1
-#define CLIMB_MODE_GAZ_BLENDED 2
-
-uint8_t climb_mode;
-uint8_t climb_gaz_submode;
-
#include "flight_plan.h"
float ground_alt;
@@ -352,11 +341,10 @@ int32_t nav_utm_north0 = NAV_UTM_NORTH0;
uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
-float desired_altitude, altitude_shift = 0;
+float altitude_shift = 0;
float desired_x, desired_y;
uint16_t nav_desired_gaz;
float nav_pitch = NAV_PITCH;
-float nav_desired_roll;
float dist2_to_wp, dist2_to_home;
bool_t too_far_from_home;
@@ -395,7 +383,7 @@ static bool_t approaching(uint8_t wp, float approaching_time) {
static inline void fly_to_xy(float x, float y) {
desired_x = x;
desired_y = y;
- desired_course = M_PI/2.-atan2(y - estimator_y, x - estimator_x);
+ h_ctl_course_setpoint = M_PI/2.-atan2(y - estimator_y, x - estimator_x);
}
/** static void fly_to(uint8_t wp)
@@ -442,12 +430,12 @@ static void route_to(uint8_t _last_wp, uint8_t wp) {
/** static void glide_to(uint8_t _last_wp, uint8_t wp)
- * \brief Computes \a nav_altitude and \a pre_climb to stay on a glide.
+ * \brief Computes \a nav_altitude and \a v_ctl_altitude_pre_climb to stay on a glide.
*/
static void glide_to(uint8_t _last_wp, uint8_t wp) {
float last_alt = waypoints[_last_wp].a;
nav_altitude = last_alt + alpha * (waypoints[wp].a - last_alt);
- pre_climb = NOMINAL_AIRSPEED * (waypoints[wp].a - last_alt) / leg;
+ v_ctl_altitude_pre_climb = NOMINAL_AIRSPEED * (waypoints[wp].a - last_alt) / leg;
}
/** \brief Compute square distance to waypoint Home and
@@ -486,6 +474,11 @@ void nav_home(void) {
void nav_update(void) {
compute_dist2_to_home();
auto_nav();
+ h_ctl_course_pre_bank = in_circle ? circle_bank : 0;
+#ifdef AGR_CLIMB
+ if ( vertical_mode == VERTICAL_MODE_AUTO_CLIMB)
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
+#endif
}
@@ -496,8 +489,6 @@ void nav_init(void) {
nav_block = 0;
nav_stage = 0;
ground_alt = GROUND_ALT;
- climb_mode = CLIMB_MODE_GAZ;
- climb_gaz_submode = CLIMB_MODE_GAZ_STANDARD;
}
/** void nav_wihtout_gps(void)
@@ -510,146 +501,15 @@ void nav_init(void) {
void nav_without_gps(void) {
#ifdef SECTION_FAILSAFE
lateral_mode = LATERAL_MODE_ROLL;
- nav_desired_roll = FAILSAFE_DEFAULT_ROLL;
+ h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
nav_pitch = FAILSAFE_DEFAULT_PITCH;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
nav_desired_gaz = TRIM_UPPRZ((FAILSAFE_DEFAULT_GAZ)*MAX_PPRZ);
#else
lateral_mode = LATERAL_MODE_ROLL;
- nav_desired_roll = 0;
+ h_ctl_roll_setpoint = 0;
nav_pitch = 0;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
- nav_desired_gaz = TRIM_UPPRZ((CLIMB_LEVEL_GAZ)*MAX_PPRZ);
-#endif
-}
-
-float course_pgain = COURSE_PGAIN;
-float desired_course = 0.;
-float max_roll = MAX_ROLL;
-float altitude_error;
-
-
-
-/** \brief Computes ::nav_desired_roll from course estimation and expected
- course.
-*/
-void course_pid_run( void ) {
- float err = estimator_hspeed_dir - desired_course;
- NormRadAngle(err);
- float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
- speed_depend_nav = Max(speed_depend_nav,0.66);
- speed_depend_nav = Min(speed_depend_nav,1.5);
- float roll_from_err = course_pgain * speed_depend_nav * err;
-#if defined AGR_CLIMB_GAZ
- if (climb_gaz_submode == CLIMB_MODE_GAZ_AGRESSIVE) {
- if (altitude_error < 0)
- roll_from_err *= AGR_CLIMB_NAV_RATIO;
- else
- roll_from_err *= AGR_DESCENT_NAV_RATIO;
- }
-#endif
- nav_desired_roll = (in_circle ? circle_bank : 0) + roll_from_err;
- if (nav_desired_roll > max_roll)
- nav_desired_roll = max_roll;
- else if (nav_desired_roll < -max_roll)
- nav_desired_roll = -max_roll;
-}
-
-#define MAX_CLIMB_SUM_ERR 150
-#define MAX_PITCH_CLIMB_SUM_ERR 100
-
-float climb_pgain = CLIMB_PGAIN;
-float climb_igain = CLIMB_IGAIN;
-float desired_climb = 0., pre_climb = 0.;
-float climb_sum_err = 0.;
-
-float climb_pitch_pgain = CLIMB_PITCH_PGAIN;
-float climb_pitch_igain = CLIMB_PITCH_IGAIN;
-float climb_pitch_sum_err = 0.;
-float climb_level_gaz = CLIMB_LEVEL_GAZ;
-
-/** \brief Computes desired_gaz and updates nav_pitch from desired_climb */
-void
-climb_pid_run ( void ) {
- float fgaz = 0;
- float err = estimator_z_dot - desired_climb;
- float controlled_gaz = climb_pgain * (err + climb_igain * climb_sum_err) + climb_level_gaz + CLIMB_GAZ_OF_CLIMB*desired_climb;
-
- /* pitch offset for climb */
- pitch_of_vz = desired_climb * pitch_of_vz_pgain;
- switch (climb_mode) {
- case CLIMB_MODE_GAZ:
-#if defined AGR_CLIMB_GAZ
- switch (climb_gaz_submode) {
- case CLIMB_MODE_GAZ_AGRESSIVE:
- if (altitude_error < 0) { /* Climbing */
- fgaz = AGR_CLIMB_GAZ;
- nav_pitch = AGR_CLIMB_PITCH;
- } else { /* Going down */
- fgaz = AGR_DESCENT_GAZ;
- nav_pitch = AGR_DESCENT_PITCH;
- }
- break;
-
- case CLIMB_MODE_GAZ_BLENDED: {
- float ratio = (fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START-AGR_BLEND_END);
- fgaz = (1-ratio)*controlled_gaz;
- nav_pitch = (1-ratio)*pitch_of_vz;
- climb_sum_err += (1-ratio)*err;
- if (altitude_error < 0) {
- fgaz += ratio*AGR_CLIMB_GAZ;
- nav_pitch += ratio*AGR_CLIMB_PITCH;
- } else {
- fgaz += ratio*AGR_DESCENT_GAZ;
- nav_pitch += ratio*AGR_DESCENT_PITCH;
- }
- break;
- }
-
- case CLIMB_MODE_GAZ_STANDARD:
-#endif
-
- fgaz = controlled_gaz;
- climb_sum_err += err;
- Bound(climb_sum_err, -MAX_CLIMB_SUM_ERR, MAX_CLIMB_SUM_ERR);
- nav_pitch += pitch_of_vz;
-
-#if defined AGR_CLIMB_GAZ
- break;
- } /* switch submode */
-#endif
- desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
- return;
-
- case CLIMB_MODE_PITCH:
- desired_gaz = nav_desired_gaz;
- nav_pitch = climb_pitch_pgain * (err + climb_pitch_igain * climb_pitch_sum_err);
- Bound(nav_pitch, MIN_PITCH, MAX_PITCH);
- climb_pitch_sum_err += err;
- BoundAbs(climb_pitch_sum_err, MAX_PITCH_CLIMB_SUM_ERR);
- return;
- }
-}
-
-float altitude_pgain = ALTITUDE_PGAIN;
-
-/** Computes desired_altitude and climb_gaz_submode */
-void altitude_pid_run(void) {
- desired_altitude = nav_altitude + altitude_shift;
- altitude_error = estimator_z - desired_altitude;
-
- desired_climb = pre_climb + altitude_pgain * altitude_error;
- Bound(desired_climb, -CLIMB_MAX, CLIMB_MAX);
-
-#ifdef AGR_CLIMB_GAZ
- if (climb_mode == CLIMB_MODE_GAZ) {
- float dist = fabs(altitude_error);
- if (dist < AGR_BLEND_END) {
- climb_gaz_submode = CLIMB_MODE_GAZ_STANDARD;
- } else if (dist > AGR_BLEND_START)
- climb_gaz_submode = CLIMB_MODE_GAZ_AGRESSIVE;
- else
- climb_gaz_submode = CLIMB_MODE_GAZ_BLENDED;
- }
+ nav_desired_gaz = TRIM_UPPRZ((CRUISE_THROTTLE)*MAX_PPRZ);
#endif
}
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index 99034b3aad..082bfff5be 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -57,7 +57,7 @@ extern const uint8_t nb_waypoint;
extern struct point waypoints[]; /** size == nb_waypoint + 1 */
extern bool_t moved_waypoints[]; /** size == nb_waypoint + 1 */
-extern float desired_altitude, desired_x, desired_y, altitude_shift;
+extern float desired_x, desired_y, altitude_shift, nav_altitude;
extern uint16_t nav_desired_gaz;
extern float nav_pitch, rc_pitch;
@@ -72,7 +72,6 @@ extern float stage_time_ds;
/** One full circle == 1. */
extern float circle_count;
-extern float nav_desired_roll;
extern float carrot_x, carrot_y;
extern bool_t in_circle;
@@ -81,7 +80,6 @@ extern int16_t circle_x, circle_y, circle_radius;
extern int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
extern uint8_t horizontal_mode;
-extern uint8_t climb_gaz_submode;
#define HORIZONTAL_MODE_WAYPOINT 0
#define HORIZONTAL_MODE_ROUTE 1
@@ -96,28 +94,10 @@ extern uint8_t climb_gaz_submode;
} \
}
-extern float course_pgain;
-extern float desired_course;
-void course_pid_run( void );
-
-extern float climb_pgain;
-extern float climb_igain;
-extern float climb_sum_err;
-extern float desired_climb, pre_climb;
-extern float altitude_pgain;
-extern float climb_pitch_pgain;
-extern float climb_pitch_igain;
-
-extern float pitch_of_vz_pgain;
-extern float pitch_of_vz;
-extern float aileron_of_gaz;
-extern float climb_level_gaz;
extern float ground_alt;
extern float survey_west, survey_east, survey_north, survey_south;
-void climb_pid_run(void);
-void altitude_pid_run(void);
void nav_update(void);
void nav_home(void);
diff --git a/sw/airborne/pid.c b/sw/airborne/pid.c
deleted file mode 100644
index 933da70666..0000000000
--- a/sw/airborne/pid.c
+++ /dev/null
@@ -1,168 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file pid.c
- * \brief PID controllers (roll, pitch, climb, altitude, course).
- *
- */
-
-#include
-#include
-
-#include "pid.h"
-
-#include "autopilot.h"
-#include "infrared.h"
-#include "gyro.h"
-#include "estimator.h"
-#include "nav.h"
-
-/* roll loop parameters */
-float desired_roll;
-float roll_pgain;
-pprz_t desired_aileron;
-
-/* pitch loop parameters */
-float desired_pitch;
-float pitch_pgain;
-pprz_t desired_elevator;
-
-/* pre-command */
-float pitch_of_roll;
-float pitch_of_vz_pgain;
-float pitch_of_vz;
-float aileron_of_gaz;
-
-pprz_t desired_gaz;
-
-#ifdef PID_RATE_LOOP
-float alt_roll_pgain; // alternate roll pgain
-float roll_rate_pgain;
-float roll_rate_dgain;
-float roll_rate_igain;
-float rate_mode;
-#ifndef RATE_MODE_DEFAULT
-#define RATE_MODE_DEFAULT 1
-#endif
-#endif
-
-inline static void pid_roll_loop_run(void);
-inline static void pid_pitch_loop_run(void);
-
-void pid_init( void ) {
- desired_roll = 0.;
- roll_pgain = ROLL_PGAIN;
-
- desired_pitch = 0.;
- pitch_pgain = PITCH_PGAIN;
-
- pitch_of_roll = PITCH_OF_ROLL;
- pitch_of_vz = 0.;
- pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN;
- aileron_of_gaz = AILERON_OF_GAZ;
-
- desired_gaz = 0;
- desired_aileron = 0;
- desired_elevator = 0;
-
-#ifdef PID_RATE_LOOP
- alt_roll_pgain = ALT_ROLL_PGAIN;
- roll_rate_pgain = ROLL_RATE_PGAIN;
- roll_rate_igain = ROLL_RATE_IGAIN;
- roll_rate_dgain = ROLL_RATE_DGAIN;
- rate_mode = RATE_MODE_DEFAULT;
-#endif
-
-}
-
-/** \brief Computes ::desired_aileron and ::desired_elevator from attitude
- * estimation and expected attitude.
-*/
-void pid_attitude_loop_run( void ) {
- pid_roll_loop_run();
- pid_pitch_loop_run();
-}
-
-
-inline static void pid_roll_loop_run(void) {
- /** Attitude PID */
- float err = estimator_phi - desired_roll;
- Bound(err, -M_PI/2., M_PI/2);
-
- float std_desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz);
-#ifndef PID_RATE_LOOP
- desired_aileron = std_desired_aileron;
-#else
- float desired_roll_rate = -alt_roll_pgain*err;
- Bound(desired_roll_rate, -GYRO_MAX_RATE, GYRO_MAX_RATE);
-
- /** Roll rate pid */
- err = roll_rate - desired_roll_rate;
- Bound(err, -GYRO_MAX_RATE, GYRO_MAX_RATE);
-
- static float rollrate_sum_err = 0.;
- static uint8_t rollratesum_idx = 0;
- static float rollratesum_values[ROLLRATESUM_NB_SAMPLES];
-
- rollrate_sum_err -= rollratesum_values[rollratesum_idx];
- rollratesum_values[rollratesum_idx] = err;
- rollrate_sum_err += err;
- rollratesum_idx++;
- if (rollratesum_idx >= ROLLRATESUM_NB_SAMPLES) rollratesum_idx = 0;
-
- /* D term calculations */
- static float last_roll_rate;
- float d_err = roll_rate - last_roll_rate;
- last_roll_rate = roll_rate;
-
- float rate_desired_aileron = TRIM_PPRZ(roll_rate_pgain*(err + roll_rate_igain*(rollrate_sum_err/ROLLRATESUM_NB_SAMPLES) + roll_rate_dgain*d_err));
-
- desired_aileron = rate_mode*rate_desired_aileron+(1-rate_mode)*std_desired_aileron;
-#endif
-
-}
-
-/** Pitch loop */
-inline static void pid_pitch_loop_run(void) {
- /* sanity check */
- if (pitch_of_roll <0.)
- pitch_of_roll = 0.;
-
- float err = -(estimator_theta - desired_pitch - pitch_of_roll*fabs(estimator_phi));
- Bound(err, -M_PI/2, M_PI/2);
- desired_elevator = TRIM_PPRZ(pitch_pgain * err);
-}
-
-
-#ifndef CLIMB_MAX_DIFF_GAZ
-#define CLIMB_MAX_DIFF_GAZ 1.
-#endif
-
-void pid_slew_gaz( void ) {
- static pprz_t last_gaz;
- pprz_t diff_gaz = desired_gaz - last_gaz;
- Bound(diff_gaz, -TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ), TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ));
- desired_gaz = last_gaz + diff_gaz;
- last_gaz = desired_gaz;
-}
diff --git a/sw/airborne/pid.h b/sw/airborne/pid.h
deleted file mode 100644
index 3ddd620d74..0000000000
--- a/sw/airborne/pid.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * Paparazzi mcu0 $Id$
- *
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef PID_H
-#define PID_H
-
-#include
-#include "paparazzi.h"
-#include "inter_mcu.h"
-
-/* roll loop parameters */
-extern float desired_roll;
-extern float roll_pgain;
-extern pprz_t desired_aileron;
-
-/* pitch loop parameters */
-extern float desired_pitch;
-extern float pitch_pgain;
-extern pprz_t desired_elevator;
-
-/* pre-command */
-extern float pitch_of_roll;
-
-#ifdef PID_RATE_LOOP
-extern float alt_roll_pgain;
-extern float rate_mode;
-extern float roll_rate_pgain;
-#endif
-
-
-void pid_init( void );
-void pid_attitude_loop_run( void );
-void pid_slew_gaz( void );
-
-extern pprz_t desired_gaz;
-
-
-#endif /* PID_H */
diff --git a/sw/airborne/sim/sim_ap.c b/sw/airborne/sim/sim_ap.c
index 59017c5afa..708ba0b54a 100644
--- a/sw/airborne/sim/sim_ap.c
+++ b/sw/airborne/sim/sim_ap.c
@@ -15,7 +15,8 @@
#include "flight_plan.h"
#include "settings.h"
#include "nav.h"
-#include "pid.h"
+#include "fw_h_ctl.h"
+#include "fw_v_ctl.h"
#include "infrared.h"
#include "cam.h"
#include "commands.h"
@@ -82,7 +83,7 @@ value sim_init(value unit) {
}
#endif
- rate_mode = 0;
+ // rate_mode = 0;
z_contrast_mode = 0;
return unit;
}
diff --git a/sw/tools/fp_syntax.ml b/sw/tools/fp_syntax.ml
index 437bfa50af..8c42ab6b95 100644
--- a/sw/tools/fp_syntax.ml
+++ b/sw/tools/fp_syntax.ml
@@ -62,7 +62,7 @@ let variables = [
"FALSE";
"QFU";
"gps_mode"; "gps_utm_east"; "gps_utm_north"; "gps_utm_zone";
- "nav_utm_east0"; "nav_utm_north0"; "nav_utm_zone0"; "climb_level_gaz"; "gps_lost"
+ "nav_utm_east0"; "nav_utm_north0"; "nav_utm_zone0"; "cruise_throttle"; "gps_lost"
]
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index e9149632b6..9a8be5188f 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -161,19 +161,18 @@ let output_vmode x wp last_wp =
let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in
if pitch = "auto"
then begin
- lprintf "climb_mode = CLIMB_MODE_PITCH;\n";
+ lprintf "v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH;\n";
lprintf "nav_desired_gaz = %f;\n" (pprz_throttle (parsed_attrib x "throttle"))
end else begin
lprintf "nav_pitch = %s;\n" (parse pitch);
- lprintf "climb_mode = CLIMB_MODE_GAZ;\n";
+ lprintf "v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;\n";
end;
let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in
begin
match vmode with
"climb" ->
- lprintf "climb_gaz_submode = CLIMB_MODE_GAZ_STANDARD;\n";
lprintf "vertical_mode = VERTICAL_MODE_AUTO_CLIMB;\n";
- lprintf "desired_climb = %s;\n" (parsed_attrib x "climb")
+ lprintf "v_ctl_climb_setpoint = %s;\n" (parsed_attrib x "climb")
| "alt" ->
lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n";
let alt =
@@ -192,7 +191,7 @@ let output_vmode x wp last_wp =
then failwith "alt or waypoint required in alt vmode"
else sprintf "waypoints[%s].a" wp in
lprintf "nav_altitude = %s;\n" alt;
- lprintf "pre_climb = 0.;\n"
+ lprintf "v_ctl_altitude_pre_climb = 0.;\n"
| "xyz" -> () (** Handled in Goto3D() *)
| "glide" ->
lprintf "vertical_mode = VERTICAL_MODE_AUTO_ALT;\n";
@@ -333,7 +332,7 @@ let rec print_stage = fun index_of_waypoints sectors x ->
end;
right ();
lprintf "lateral_mode = LATERAL_MODE_ROLL;\n";
- lprintf "nav_desired_roll = RadOfDeg(%s);\n" (parsed_attrib x "roll");
+ lprintf "h_ctl_roll_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "roll");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";