[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6354] moved fw_[hv]_ctl to firmwares/fixedwing/stab
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [6354] moved fw_[hv]_ctl to firmwares/fixedwing/stabilization and guidance |
|
Date: |
Fri, 05 Nov 2010 00:40:56 +0000 |
Revision: 6354
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6354
Author: poine
Date: 2010-11-05 00:40:55 +0000 (Fri, 05 Nov 2010)
Log Message:
-----------
moved fw_[hv]_ctl to firmwares/fixedwing/stabilization and guidance
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/swift_1.xml
paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile
paparazzi3/trunk/conf/autopilot/sitl.makefile
paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile
paparazzi3/trunk/conf/autopilot/sitl_link_pprz.makefile
paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control.makefile
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control_adaptive.makefile
paparazzi3/trunk/sw/airborne/ap_downlink.h
paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h
paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c
paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c
paparazzi3/trunk/sw/airborne/joystick.h
paparazzi3/trunk/sw/airborne/modules/multi/formation.c
paparazzi3/trunk/sw/airborne/modules/multi/potential.c
paparazzi3/trunk/sw/airborne/nav.c
paparazzi3/trunk/sw/airborne/nav.h
paparazzi3/trunk/sw/airborne/rc_settings.c
Added Paths:
-----------
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_spektrum.makefile
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/fw_h_ctl.c
paparazzi3/trunk/sw/airborne/fw_h_ctl.h
paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c
paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h
paparazzi3/trunk/sw/airborne/fw_v_ctl.h
paparazzi3/trunk/sw/airborne/fw_v_ctl_n.c
paparazzi3/trunk/sw/airborne/fw_v_ctl_n.h
Modified: paparazzi3/trunk/conf/airframes/Poine/swift_1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/swift_1.xml 2010-11-04 16:18:38 UTC
(rev 6353)
+++ paparazzi3/trunk/conf/airframes/Poine/swift_1.xml 2010-11-05 00:40:55 UTC
(rev 6354)
@@ -11,8 +11,8 @@
<firmware name="fixedwing">
<target name="sim" board="pc"/>
<target name="ap" board="lisa_m_1.0"/>
-
- <subsystem name="radio_control" type="ppm"/>
+<!-- <subsystem name="radio_control" type="ppm"/> -->
+ <subsystem name="radio_control" type="spektrum"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="control"/>
<subsystem name="attitude" type="infrared"/>
@@ -20,6 +20,24 @@
<subsystem name="navigation"/>
</firmware>
+ <firmware name="lisa_l_test_progs">
+ <target name="test_led" board="lisa_m_1.0"/>
+ <target name="test_uart" board="lisa_m_1.0"/>
+ <target name="test_servos" board="lisa_m_1.0"/>
+ <target name="test_telemetry" board="lisa_m_1.0">
+ <param name="MODEM_PORT" value="UART2"/>
+ </target>
+ <target name="test_bmp085" board="lisa_m_1.0"/>
+ <target name="test_esc_mkk_simple" board="lisa_m_1.0"/>
+ <target name="test_rc_spektrum" board="lisa_m_1.0"/>
+ <target name="test_manual" board="lisa_m_1.0"/>
+ <target name="test_esc_mkk_simple" board="lisa_m_1.0"/>
+ <target name="test_hmc5843" board="lisa_m_1.0"/>
+ <target name="test_itg3200" board="lisa_m_1.0"/>
+ <target name="test_adxl345" board="lisa_m_1.0"/>
+ <target name="test_adc" board="lisa_m_1.0"/>
+ </firmware>
+
<servos>
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="1" min="1900" neutral="1521" max="1100"/>
@@ -175,24 +193,6 @@
<set servo="S6" value="@ROLL"/>
</command_laws>
- <firmware name="lisa_l_test_progs">
- <target name="test_led" board="lisa_m_1.0"/>
- <target name="test_uart" board="lisa_m_1.0"/>
- <target name="test_servos" board="lisa_m_1.0"/>
- <target name="test_telemetry" board="lisa_m_1.0">
- <param name="MODEM_PORT" value="UART2"/>
- </target>
- <target name="test_bmp085" board="lisa_m_1.0"/>
- <target name="test_esc_mkk_simple" board="lisa_m_1.0"/>
- <target name="test_rc_spektrum" board="lisa_m_1.0"/>
- <target name="test_manual" board="lisa_m_1.0"/>
- <target name="test_esc_mkk_simple" board="lisa_m_1.0"/>
- <target name="test_hmc5843" board="lisa_m_1.0"/>
- <target name="test_itg3200" board="lisa_m_1.0"/>
- <target name="test_adxl345" board="lisa_m_1.0"/>
- <target name="test_adc" board="lisa_m_1.0"/>
- </firmware>
-
</airframe>
-->
\ No newline at end of file
Modified: paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile 2010-11-05
00:40:55 UTC (rev 6354)
@@ -22,6 +22,9 @@
#
#
+
+
+
################################################################################
#
#
@@ -474,8 +477,8 @@
test_adxl345.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ)
-I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
test_adxl345.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
test_adxl345.srcs += lisa/test/lisa_test_adxl345_dma.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
test_adxl345.CFLAGS += -DUSE_LED
test_adxl345.srcs += $(SRC_ARCH)/led_hw.c
@@ -499,19 +502,19 @@
# simple test of mikrokopter motor controllers
#
test_esc_mkk_simple.ARCHDIR = $(ARCH)
-test_esc_mkk_simple.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH)
-DPERIPHERALS_AUTO_INIT
+test_esc_mkk_simple.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH)
-DPERIPHERALS_AUTO_INIT
test_esc_mkk_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_esc_mkk_simple.srcs = test/test_esc_mkk_simple.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+test_esc_mkk_simple.srcs = test/test_esc_mkk_simple.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
test_esc_mkk_simple.CFLAGS += -DUSE_LED
-test_esc_mkk_simple.srcs += $(SRC_ARCH)/led_hw.c
+test_esc_mkk_simple.srcs += $(SRC_ARCH)/led_hw.c
test_esc_mkk_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
test_esc_mkk_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_esc_mkk_simple.CFLAGS += -DUSE_I2C1
-test_esc_mkk_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c1
+test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_esc_mkk_simple.CFLAGS += -DUSE_I2C2
+test_esc_mkk_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c2
#
Modified: paparazzi3/trunk/conf/autopilot/sitl.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl.makefile 2010-11-04 16:18:38 UTC
(rev 6353)
+++ paparazzi3/trunk/conf/autopilot/sitl.makefile 2010-11-05 00:40:55 UTC
(rev 6354)
@@ -1,6 +1,27 @@
sim.ARCHDIR = $(ARCH)
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED -DWIND_INFO
-sim.srcs += latlong.c 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 sys_time.c
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c datalink.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 $(SRC_ARCH)/sim_adc_generic.c
$(SRC_ARCH)/led_hw.c
+sim.srcs += latlong.c\
+ radio_control.c\
+ downlink.c\
+ commands.c\
+ gps.c\
+ inter_mcu.c\
+ infrared.c\
+ $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c\
+ $(SRC_FIRMWARE)/guidance/guidance_v.c \
+ nav.c\
+ estimator.c\
+ sys_time.c\
+ $(SRC_FIRMWARE)/main_fbw.c \
+ $(SRC_FIRMWARE)/main_ap.c \
+ datalink.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 \
+ $(SRC_ARCH)/sim_adc_generic.c \
+ $(SRC_ARCH)/led_hw.c
Modified: paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-11-05
00:40:55 UTC (rev 6354)
@@ -26,7 +26,10 @@
jsbsim.CFLAGS += -DSITL -DAP -DFBW -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED -DWIND_INFO
-Ifirmwares/fixedwing
jsbsim.srcs = $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c
$(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_transport.c
$(SRC_ARCH)/ivy_transport.c
-jsbsim.srcs += latlong.c downlink.c commands.c gps.c inter_mcu.c infrared.c
fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c $(SRC_FIRMWARE)/main_fbw.c
$(SRC_FIRMWARE)/main_ap.c datalink.c
+jsbsim.srcs += latlong.c downlink.c commands.c gps.c inter_mcu.c infrared.c \
+ $(SRC_FIXEDWING)/stabilization/stabilization_attitude.c \
+ $(SRC_FIXEDWING)/guidance/guidance_v.c\
+ nav.c estimator.c sys_time.c $(SRC_FIRMWARE)/main_fbw.c
$(SRC_FIRMWARE)/main_ap.c datalink.c
jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c
# Choose in your airframe file type of airframe
# jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
Modified: paparazzi3/trunk/conf/autopilot/sitl_link_pprz.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_link_pprz.makefile 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/conf/autopilot/sitl_link_pprz.makefile 2010-11-05
00:40:55 UTC (rev 6354)
@@ -1,3 +1,3 @@
sim.ARCHDIR = $(ARCH)
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=PprzTransport -DINFRARED -DRADIO_CONTROL_SETTINGS
-DSIM_UART -DDOWNLINK_AP_DEVICE=SimUart -DDOWNLINK_FBW_DEVICE=SimUart
-DDATALINK=PPRZ
-sim.srcs = radio_control.c downlink.c pprz_transport.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
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/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)/sim_uart.c datalink.c
+sim.srcs = radio_control.c downlink.c pprz_transport.c commands.c gps.c
inter_mcu.c infrared.c $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c
$(SRC_FIRMWARE)/guidance/guidance_v.c nav.c estimator.c cam.c sys_time.c
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/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)/sim_uart.c datalink.c
Modified: paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile 2010-11-05
00:40:55 UTC (rev 6354)
@@ -1,3 +1,3 @@
sim.ARCHDIR = $(ARCH)
sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=XBeeTransport -DINFRARED -DRADIO_CONTROL_SETTINGS
-DSIM_UART -DSIM_XBEE -DXBEE_UART=SimUart
-sim.srcs = radio_control.c downlink.c xbee.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
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/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)/sim_uart.c
+sim.srcs = radio_control.c downlink.c xbee.c commands.c gps.c inter_mcu.c
infrared.c $(SRC_FIXEDWING)/stabilization/stabilization_attitude.c
$(SRC_FIXEDWING)/guidance/guidance_v.c nav.c estimator.c cam.c sys_time.c
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/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)/sim_uart.c
Modified: paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control.makefile
2010-11-04 16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control.makefile
2010-11-05 00:40:55 UTC (rev 6354)
@@ -1,4 +1,4 @@
# Standard fixed wing control loops
-$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl.c $(SRC_FIXEDWING)/fw_v_ctl.c
+$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c
$(SRC_FIRMWARE)/guidance/guidance_v.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control_adaptive.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control_adaptive.makefile
2010-11-04 16:18:38 UTC (rev 6353)
+++
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/control_adaptive.makefile
2010-11-05 00:40:55 UTC (rev 6354)
@@ -1,4 +1,5 @@
# fixed wing control loops with adaptive horizontal control
-$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl_a.c $(SRC_FIXEDWING)/fw_v_ctl.c
+$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c
$(SRC_FIRMWARE)/guidance/guidance_v.c
+
Added:
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_spektrum.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_spektrum.makefile
(rev 0)
+++
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_spektrum.makefile
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,21 @@
+#
+# Makefile for radio_control susbsytem in fixedwing firmware
+#
+ifndef RADIO_CONTROL_SPEKTRUM_MODEL
+RADIO_CONTROL_SPEKTRUM_MODEL=\"subsystems/radio_control/spektrum_dx7se.h\"
+endif
+
+ap.CFLAGS += -DRADIO_CONTROL
-DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
+ap.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
+ifeq ($(BOARD), booz)
+ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL)
+endif
+ifdef RADIO_CONTROL_LED
+ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+endif
+ap.CFLAGS +=
-DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER
-DUSE_TIM6_IRQ
+
+ap.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
+ $(SRC_SUBSYSTEMS)/radio_control/spektrum.c \
+ $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
Modified: paparazzi3/trunk/sw/airborne/ap_downlink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/ap_downlink.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/ap_downlink.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -205,7 +205,7 @@
#define PERIODIC_SEND_ENERGY(_chan) Downlink({ const int16_t e = energy; const
float vsup = ((float)vsupply) / 10.0f; const float curs = ((float)
current)/1000.0f; const float power = vsup * curs; DOWNLINK_SEND_ENERGY(_chan,
&vsup, &curs, &e, &power); })
-#include "fw_h_ctl_a.h"
+#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
#define PERIODIC_SEND_H_CTL_A(_chan) DOWNLINK_SEND_H_CTL_A(_chan,
&h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err,
&h_ctl_ref_pitch_angle)
Modified: paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h 2010-11-04 16:18:38 UTC
(rev 6353)
+++ paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h 2010-11-05 00:40:55 UTC
(rev 6354)
@@ -38,8 +38,8 @@
#include "airframe.h"
#include "settings.h"
#include "nav.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "infrared.h"
#include "commands.h"
#include "firmwares/fixedwing/main_ap.h"
Modified: paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c 2010-11-04 16:18:38 UTC
(rev 6353)
+++ paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c 2010-11-05 00:40:55 UTC
(rev 6354)
@@ -16,8 +16,8 @@
#include "flight_plan.h"
#include "settings.h"
#include "nav.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "infrared.h"
#include "commands.h"
#include "firmwares/fixedwing/main_ap.h"
Modified: paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c 2010-11-04 16:18:38 UTC
(rev 6353)
+++ paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c 2010-11-05 00:40:55 UTC
(rev 6354)
@@ -16,8 +16,8 @@
#include "flight_plan.h"
#include "settings.h"
#include "nav.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "ffirmwares/fixedwing/guidance.h"
#include "infrared.h"
#include "commands.h"
#include "firmwares/fixedwing/main_ap.h"
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_v_ctl.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,379 @@
+/*
+ * $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 "firmwares/fixedwing/guidance.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+#include "firmwares/fixedwing/autopilot.h"
+
+/* mode */
+uint8_t v_ctl_mode;
+
+/* 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_nominal_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_dgain;
+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;
+float v_ctl_auto_throttle_pitch_of_vz_dgain;
+
+#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
+#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
+#endif
+
+/* agressive tuning */
+#ifdef TUNE_AGRESSIVE_CLIMB
+float agr_climb_throttle;
+float agr_climb_pitch;
+float agr_climb_nav_ratio;
+float agr_descent_throttle;
+float agr_descent_pitch;
+float agr_descent_nav_ratio;
+#endif
+
+/* "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;
+pprz_t v_ctl_throttle_slewed;
+
+inline static void v_ctl_climb_auto_throttle_loop( void );
+#ifdef V_CTL_AUTO_PITCH_PGAIN
+inline static void v_ctl_climb_auto_pitch_loop( void );
+#endif
+
+#ifdef USE_AIRSPEED
+float v_ctl_auto_airspeed_setpoint;
+float v_ctl_auto_airspeed_controlled;
+float v_ctl_auto_airspeed_pgain;
+float v_ctl_auto_airspeed_igain;
+float v_ctl_auto_airspeed_sum_err;
+float v_ctl_auto_groundspeed_setpoint;
+float v_ctl_auto_groundspeed_pgain;
+float v_ctl_auto_groundspeed_igain;
+float v_ctl_auto_groundspeed_sum_err;
+#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
+#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
+#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
+#define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in
aggressive climb mode
+#endif
+
+
+void v_ctl_init( void ) {
+ /* mode */
+ v_ctl_mode = V_CTL_MODE_MANUAL;
+
+ /* 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;
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
+
+ /* "auto throttle" inner loop parameters */
+ v_ctl_auto_throttle_nominal_cruise_throttle =
V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+ v_ctl_auto_throttle_cruise_throttle =
v_ctl_auto_throttle_nominal_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_dgain = 0.;
+ v_ctl_auto_throttle_sum_err = 0.;
+ v_ctl_auto_throttle_pitch_of_vz_pgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
+ v_ctl_auto_throttle_pitch_of_vz_dgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN;
+
+#ifdef V_CTL_AUTO_PITCH_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.;
+#endif
+
+#ifdef USE_AIRSPEED
+ v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
+ v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
+ v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
+ v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
+ v_ctl_auto_airspeed_sum_err = 0.;
+
+ v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
+ v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
+ v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
+ v_ctl_auto_groundspeed_sum_err = 0.;
+#endif
+
+ v_ctl_throttle_setpoint = 0;
+
+/*agressive tuning*/
+#ifdef TUNE_AGRESSIVE_CLIMB
+ agr_climb_throttle = AGR_CLIMB_THROTTLE;
+ #undef AGR_CLIMB_THROTTLE
+ #define AGR_CLIMB_THROTTLE agr_climb_throttle
+ agr_climb_pitch = AGR_CLIMB_PITCH;
+ #undef AGR_CLIMB_PITCH
+ #define AGR_CLIMB_PITCH agr_climb_pitch
+ agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO;
+ #undef AGR_CLIMB_NAV_RATIO
+ #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio
+ agr_descent_throttle = AGR_DESCENT_THROTTLE;
+ #undef AGR_DESCENT_THROTTLE
+ #define AGR_DESCENT_THROTTLE agr_descent_throttle
+ agr_descent_pitch = AGR_DESCENT_PITCH;
+ #undef AGR_DESCENT_PITCH
+ #define AGR_DESCENT_PITCH agr_descent_pitch
+ agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO;
+ #undef AGR_DESCENT_NAV_RATIO
+ #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio
+#endif
+}
+
+/**
+ * outer loop
+ * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
+ */
+void v_ctl_altitude_loop( void ) {
+ float altitude_pgain_boost = 1.0;
+
+#if defined(USE_AIRSPEED) && defined(AGR_CLIMB)
+ // Aggressive climb mode (boost gain of altitude loop)
+ if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
+ float dist = fabs(v_ctl_altitude_error);
+ altitude_pgain_boost = 1.0 +
(V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END);
+ Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
+ }
+#endif
+
+ v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
+ v_ctl_climb_setpoint = altitude_pgain_boost * 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:
+ default:
+ v_ctl_climb_auto_throttle_loop();
+ break;
+#ifdef V_CTL_AUTO_PITCH_PGAIN
+ case V_CTL_CLIMB_MODE_AUTO_PITCH:
+ v_ctl_climb_auto_pitch_loop();
+ break;
+#endif
+ }
+}
+
+/**
+ * auto throttle inner loop
+ * \brief
+ */
+
+#ifndef USE_AIRSPEED
+
+inline static void v_ctl_climb_auto_throttle_loop(void) {
+ static float last_err;
+
+ float f_throttle = 0;
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float d_err = err - last_err;
+ last_err = err;
+ 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
+ + v_ctl_auto_throttle_dgain * d_err);
+
+ /* pitch pre-command */
+ float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err *
v_ctl_auto_throttle_pitch_of_vz_dgain) * 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;
+ BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_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:
+ default:
+#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);
+}
+
+#else // USE_AIRSPEED
+
+inline static void v_ctl_climb_auto_throttle_loop(void) {
+ float f_throttle = 0;
+ float controlled_throttle;
+ float v_ctl_pitch_of_vz;
+
+ // Limit rate of change of climb setpoint (to ensure that airspeed loop can
catch-up)
+ static float v_ctl_climb_setpoint_last = 0;
+ float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
+ Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
+ v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
+ v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
+
+ // Pitch control (input: rate of climb error, output: pitch setpoint)
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ v_ctl_auto_pitch_sum_err += err;
+ BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
+ v_ctl_pitch_of_vz = v_ctl_auto_pitch_pgain *
+ (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
+
+ // Ground speed control loop (input: groundspeed error, output: airspeed
controlled)
+ float err_groundspeed = (v_ctl_auto_groundspeed_setpoint -
estimator_hspeed_mod);
+ v_ctl_auto_groundspeed_sum_err += err_groundspeed;
+ BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
+ v_ctl_auto_airspeed_controlled = (err_groundspeed +
v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
v_ctl_auto_groundspeed_pgain;
+
+ // Do not allow controlled airspeed below the setpoint
+ if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
+ v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
+ v_ctl_auto_groundspeed_sum_err =
v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain);
// reset integrator of ground speed loop
+ }
+
+ // Airspeed control loop (input: airspeed controlled, output: throttle
controlled)
+ float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed);
+ v_ctl_auto_airspeed_sum_err += err_airspeed;
+ BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
+ controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err *
v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
+
+ // Done, set outputs
+ Bound(controlled_throttle, 0, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+ f_throttle = controlled_throttle;
+ nav_pitch = v_ctl_pitch_of_vz;
+ v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
+ Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
+}
+
+#endif // USE_AIRSPEED
+
+
+/**
+ * auto pitch inner loop
+ * \brief computes a nav_pitch from a climb_setpoint given a fixed throttle
+ */
+#ifdef V_CTL_AUTO_PITCH_PGAIN
+inline static void v_ctl_climb_auto_pitch_loop(void) {
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ v_ctl_throttle_setpoint = nav_throttle_setpoint;
+ 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);
+}
+#endif
+
+#ifdef V_CTL_THROTTLE_SLEW_LIMITER
+#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
+#endif
+
+#ifndef V_CTL_THROTTLE_SLEW
+#define V_CTL_THROTTLE_SLEW 1.
+#endif
+/** \brief Computes slewed throttle from throttle setpoint
+ called at 20Hz
+ */
+void v_ctl_throttle_slew( void ) {
+ pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
+ BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
+ v_ctl_throttle_slewed += diff_throttle;
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_v_ctl.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,126 @@
+/*
+ * 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 <inttypes.h>
+#include "paparazzi.h"
+
+/* Vertical mode */
+#define V_CTL_MODE_MANUAL 0
+#define V_CTL_MODE_AUTO_THROTTLE 1
+#define V_CTL_MODE_AUTO_CLIMB 2
+#define V_CTL_MODE_AUTO_ALT 3
+#define V_CTL_MODE_NB 4
+extern uint8_t v_ctl_mode;
+
+/* outer loop */
+extern float v_ctl_altitude_error;
+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
+
+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
+
+/* "auto throttle" inner loop parameters */
+extern float v_ctl_auto_throttle_nominal_cruise_throttle;
+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_dgain;
+extern float v_ctl_auto_throttle_sum_err;
+extern float v_ctl_auto_throttle_pitch_of_vz_pgain;
+extern float v_ctl_auto_throttle_pitch_of_vz_dgain;
+
+#ifdef LOITER_TRIM
+extern float v_ctl_auto_throttle_loiter_trim;
+extern float v_ctl_auto_throttle_dash_trim;
+#endif
+
+/* agressive tuning */
+#ifdef TUNE_AGRESSIVE_CLIMB
+extern float agr_climb_throttle;
+extern float agr_climb_pitch;
+extern float agr_climb_nav_ratio;
+extern float agr_descent_throttle;
+extern float agr_descent_pitch;
+extern float agr_descent_nav_ratio;
+#endif
+
+/* "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 pprz_t v_ctl_throttle_slewed;
+
+extern void v_ctl_init( void );
+extern void v_ctl_altitude_loop( void );
+extern void v_ctl_climb_loop ( void );
+
+#ifdef USE_AIRSPEED
+/* "airspeed" inner loop parameters */
+extern float v_ctl_auto_airspeed_setpoint;
+extern float v_ctl_auto_airspeed_controlled;
+extern float v_ctl_auto_airspeed_pgain;
+extern float v_ctl_auto_airspeed_igain;
+extern float v_ctl_auto_airspeed_sum_err;
+extern float v_ctl_auto_groundspeed_setpoint;
+extern float v_ctl_auto_groundspeed_pgain;
+extern float v_ctl_auto_groundspeed_igain;
+extern float v_ctl_auto_groundspeed_sum_err;
+#endif
+
+/** Computes throttle_slewed from throttle_setpoint */
+extern void v_ctl_throttle_slew( void );
+
+#define fw_v_ctl_SetCruiseThrottle(_v) { \
+ v_ctl_auto_throttle_cruise_throttle = (_v ? _v :
v_ctl_auto_throttle_nominal_cruise_throttle); \
+ Bound(v_ctl_auto_throttle_cruise_throttle,
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE,
V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); \
+}
+
+#define fw_v_ctl_SetAutoThrottleIgain(_v) { \
+ v_ctl_auto_throttle_igain = _v; \
+ v_ctl_auto_throttle_sum_err = 0; \
+ }
+
+#endif /* FW_V_CTL_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c (from
rev 6352, paparazzi3/trunk/sw/airborne/fw_v_ctl_n.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,292 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2010 ENAC
+ *
+ * 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_n
+ * \brief Vertical control for fixed wing vehicles.
+ *
+ */
+
+#include "firmwares/fixedwing/guidance/guidance_v.h"
+#include "firmwares/fixedwing/guidance/guidance_v_n.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+#include "autopilot.h"
+
+/* mode */
+uint8_t v_ctl_mode;
+
+/* 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_nominal_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_dgain;
+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;
+float v_ctl_auto_throttle_pitch_of_vz_dgain;
+
+#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
+#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
+#endif
+
+/* "auto pitch" inner loop parameters */
+float v_ctl_auto_pitch_pgain;
+float v_ctl_auto_pitch_dgain;
+float v_ctl_auto_pitch_igain;
+float v_ctl_auto_pitch_sum_err;
+#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
+
+float controlled_throttle;
+pprz_t v_ctl_throttle_setpoint;
+pprz_t v_ctl_throttle_slewed;
+
+// Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
+#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
+
+uint8_t v_ctl_speed_mode;
+
+#ifdef USE_AIRSPEED
+float v_ctl_auto_airspeed_setpoint;
+float v_ctl_auto_airspeed_controlled;
+float v_ctl_auto_airspeed_pgain;
+float v_ctl_auto_airspeed_igain;
+float v_ctl_auto_airspeed_sum_err;
+float v_ctl_auto_groundspeed_setpoint;
+float v_ctl_auto_groundspeed_pgain;
+float v_ctl_auto_groundspeed_igain;
+float v_ctl_auto_groundspeed_sum_err;
+#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
+#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
+#endif
+
+
+void v_ctl_init( void ) {
+ /* mode */
+ v_ctl_mode = V_CTL_MODE_MANUAL;
+ v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
+
+ /* 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;
+ v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
+
+ /* "auto throttle" inner loop parameters */
+ v_ctl_auto_throttle_nominal_cruise_throttle =
V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+ v_ctl_auto_throttle_cruise_throttle =
v_ctl_auto_throttle_nominal_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_dgain = 0.;
+ v_ctl_auto_throttle_sum_err = 0.;
+ v_ctl_auto_throttle_pitch_of_vz_pgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
+ v_ctl_auto_throttle_pitch_of_vz_dgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN;
+
+ /* "auto pitch" inner loop parameters */
+ v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
+ v_ctl_auto_pitch_dgain = V_CTL_AUTO_PITCH_DGAIN;
+ v_ctl_auto_pitch_igain = V_CTL_AUTO_PITCH_IGAIN;
+ v_ctl_auto_pitch_sum_err = 0.;
+
+#ifdef USE_AIRSPEED
+ v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
+ v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
+ v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
+ v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
+ v_ctl_auto_airspeed_sum_err = 0.;
+
+ v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
+ v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
+ v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
+ v_ctl_auto_groundspeed_sum_err = 0.;
+#endif
+
+ controlled_throttle = 0.;
+ v_ctl_throttle_setpoint = 0;
+}
+
+/**
+ * outer loop
+ * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
+ */
+
+// Don't use lead controller unless you know what you're doing
+#define LEAD_CTRL_TAU 0.8
+#define LEAD_CTRL_A 1.
+#define LEAD_CTRL_Te (1./60.)
+
+void v_ctl_altitude_loop( void ) {
+ static float v_ctl_climb_setpoint_last = 0.;
+ //static float last_lead_input = 0.;
+
+ // Altitude error
+ 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;
+
+ // Lead controller
+ //v_ctl_climb_setpoint = ((LEAD_CTRL_TAU*LEAD_CTRL_A +
LEAD_CTRL_Te)*climb_sp + LEAD_CTRL_TAU*(v_ctl_climb_setpoint -
LEAD_CTRL_A*last_lead_input)) / (LEAD_CTRL_TAU + LEAD_CTRL_Te);
+ //last_lead_input = pitch_sp;
+
+ // Limit rate of change of climb setpoint
+ float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
+ BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT);
+ v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
+
+ // Limit climb setpoint
+ BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
+ v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
+}
+
+static inline void v_ctl_set_pitch ( void ) {
+ static float last_err = 0.;
+
+ // Compute errors
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float d_err = err - last_err;
+ last_err = err;
+
+ v_ctl_auto_pitch_sum_err += err*(1./60.);
+ BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
+
+ // PI loop + feedforward ctl
+ nav_pitch = nav_pitch
+ + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint
+ + v_ctl_auto_pitch_pgain * err
+ + v_ctl_auto_pitch_dgain * d_err
+ + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err;
+
+}
+
+static inline void v_ctl_set_throttle( void ) {
+ static float last_err = 0.;
+
+ // Compute errors
+ float err = estimator_z_dot - v_ctl_climb_setpoint;
+ float d_err = err - last_err;
+ last_err = err;
+
+ v_ctl_auto_throttle_sum_err += err*(1./60.);
+ BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
+
+ // PID loop + feedforward ctl
+ 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_dgain * d_err
+ + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err;
+
+}
+
+#ifdef USE_AIRSPEED
+static inline void v_ctl_set_airspeed( void ) {
+ // Bound airspeed setpoint
+ Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
+
+ // Airspeed control loop (input: airspeed controlled, output: throttle
controlled)
+ float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
+ v_ctl_auto_airspeed_sum_err += err_airspeed;
+ BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
+ controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ + v_ctl_auto_airspeed_pgain * err_airspeed
+ + v_ctl_auto_airspeed_igain * v_ctl_auto_airspeed_sum_err;
+
+}
+
+static inline void v_ctl_set_groundspeed( void ) {
+ // Ground speed control loop (input: groundspeed error, output: airspeed
controlled)
+ float err_groundspeed = v_ctl_auto_groundspeed_setpoint -
estimator_hspeed_mod;
+ v_ctl_auto_groundspeed_sum_err += err_groundspeed;
+ BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
+ v_ctl_auto_airspeed_setpoint = err_groundspeed *
v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err *
v_ctl_auto_groundspeed_igain;
+
+}
+#endif
+
+void v_ctl_climb_loop ( void ) {
+
+ // Set pitch
+ v_ctl_set_pitch();
+ Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
+
+ // Set throttle
+ switch (v_ctl_speed_mode) {
+ case V_CTL_SPEED_THROTTLE:
+ v_ctl_set_throttle();
+ break;
+#ifdef USE_AIRSPEED
+ case V_CTL_SPEED_AIRSPEED:
+ v_ctl_set_airspeed();
+ break;
+ case V_CTL_SPEED_GROUNDSPEED:
+ v_ctl_set_groundspeed();
+ v_ctl_set_airspeed();
+ break;
+#endif
+ default:
+ controlled_throttle = v_ctl_auto_throttle_cruise_throttle; // ???
+ break;
+ }
+
+ // Set Throttle output
+ float f_throttle = controlled_throttle;
+ v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
+
+}
+
+#ifdef V_CTL_THROTTLE_SLEW_LIMITER
+#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
+#endif
+
+#ifndef V_CTL_THROTTLE_SLEW
+#define V_CTL_THROTTLE_SLEW 1.
+#endif
+/** \brief Computes slewed throttle from throttle setpoint
+ called at 20Hz
+ */
+void v_ctl_throttle_slew( void ) {
+ pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
+ BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
+ v_ctl_throttle_slewed += diff_throttle;
+}
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h (from
rev 6352, paparazzi3/trunk/sw/airborne/fw_v_ctl_n.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,47 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2010 ENAC
+ *
+ * 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_N_H
+#define FW_V_CTL_N_H
+
+#define V_CTL_SPEED_THROTTLE 0
+#define V_CTL_SPEED_AIRSPEED 1
+#define V_CTL_SPEED_GROUNDSPEED 2
+
+extern float v_ctl_auto_pitch_dgain;
+
+extern uint8_t v_ctl_speed_mode;
+
+#ifdef PITCH_TRIM
+extern float v_ctl_pitch_loiter_trim;
+extern float v_ctl_pitch_dash_trim;
+#endif
+
+#endif /* FW_V_CTL_N_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c 2010-11-05
00:40:55 UTC (rev 6354)
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2003-2010 Paparazzi team
+ * Copyright (C) 2003-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -38,8 +38,8 @@
#include "interrupt_hw.h"
#include "init_hw.h"
#include "adc.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "gps.h"
#include "infrared.h"
#include "gyro.h"
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,345 @@
+/*
+ * Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $
+ *
+ * Copyright (C) 2009-2010 The Paparazzi Team
+ *
+ * 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 adaptive control
+ *
+ */
+
+#include "std.h"
+#include "led.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+#include "firmwares/fixedwing/guidance.h"
+#include "autopilot.h"
+
+
+/* outer loop parameters */
+float h_ctl_course_setpoint; /* rad, CW/north */
+float h_ctl_course_pre_bank;
+float h_ctl_course_pre_bank_correction;
+float h_ctl_course_pgain;
+float h_ctl_course_dgain;
+float h_ctl_roll_max_setpoint;
+
+/* roll and pitch disabling */
+bool_t h_ctl_disabled;
+
+/* AUTO1 rate mode */
+bool_t h_ctl_auto1_rate;
+
+
+float h_ctl_ref_roll_angle;
+float h_ctl_ref_roll_rate;
+float h_ctl_ref_roll_accel;
+float h_ctl_ref_pitch_angle;
+float h_ctl_ref_pitch_rate;
+float h_ctl_ref_pitch_accel;
+#define H_CTL_REF_W 2.5
+#define H_CTL_REF_XI 0.85
+#define H_CTL_REF_MAX_P RadOfDeg(100.)
+#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
+#define H_CTL_REF_MAX_Q RadOfDeg(100.)
+#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
+
+/* inner roll loop parameters */
+float h_ctl_roll_setpoint;
+float h_ctl_roll_attitude_gain;
+float h_ctl_roll_rate_gain;
+float h_ctl_roll_igain;
+float h_ctl_roll_sum_err;
+float h_ctl_roll_Kffa;
+float h_ctl_roll_Kffd;
+pprz_t h_ctl_aileron_setpoint;
+float h_ctl_roll_slew;
+
+float h_ctl_roll_pgain;
+
+/* inner pitch loop parameters */
+float h_ctl_pitch_setpoint;
+float h_ctl_pitch_loop_setpoint;
+float h_ctl_pitch_pgain;
+float h_ctl_pitch_dgain;
+float h_ctl_pitch_igain;
+float h_ctl_pitch_sum_err;
+float h_ctl_pitch_Kffa;
+float h_ctl_pitch_Kffd;
+pprz_t h_ctl_elevator_setpoint;
+
+/* inner loop pre-command */
+float h_ctl_aileron_of_throttle;
+float h_ctl_elevator_of_roll;
+float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
+
+
+inline static void h_ctl_roll_loop( void );
+inline static void h_ctl_pitch_loop( void );
+
+#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
+#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
+#endif
+
+#ifndef H_CTL_COURSE_DGAIN
+#define H_CTL_COURSE_DGAIN 0.
+#endif
+
+#ifndef H_CTL_ROLL_RATE_GAIN
+#define H_CTL_ROLL_RATE_GAIN 0.
+#endif
+
+void h_ctl_init( void ) {
+ h_ctl_course_setpoint = 0.;
+ h_ctl_course_pre_bank = 0.;
+ h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
+ h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
+ h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
+ h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
+
+ h_ctl_disabled = FALSE;
+
+ h_ctl_roll_setpoint = 0.;
+ h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
+ h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
+ h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
+ h_ctl_roll_sum_err = 0;
+ h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
+ h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
+ h_ctl_aileron_setpoint = 0;
+#ifdef H_CTL_AILERON_OF_THROTTLE
+ h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
+#endif
+
+ h_ctl_pitch_setpoint = 0.;
+ h_ctl_pitch_loop_setpoint = 0.;
+ h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
+ h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
+ h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
+ h_ctl_pitch_sum_err = 0.;
+ h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
+ h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
+ h_ctl_elevator_setpoint = 0;
+ h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
+#ifdef H_CTL_PITCH_OF_ROLL
+ h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
+#endif
+
+}
+
+/**
+ * \brief
+ *
+ */
+void h_ctl_course_loop ( void ) {
+ static float last_err;
+
+ // Ground path error
+ float err = estimator_hspeed_dir - h_ctl_course_setpoint;
+ NormRadAngle(err);
+
+ float d_err = err - last_err;
+ last_err = err;
+ NormRadAngle(d_err);
+
+ float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+ Bound(speed_depend_nav, 0.66, 1.5);
+
+ h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction *
h_ctl_course_pre_bank
+ + h_ctl_course_pgain * speed_depend_nav * err
+ + h_ctl_course_dgain * d_err;
+
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
+}
+
+static float airspeed_ratio2;
+
+static inline void compute_airspeed_ratio( void ) {
+ float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ -
v_ctl_auto_throttle_nominal_cruise_throttle;
+ float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */
+ if (throttle_diff > 0)
+ airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED -
NOMINAL_AIRSPEED);
+ else
+ airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle
- V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED -
MINIMUM_AIRSPEED);
+
+ float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
+ Bound(airspeed_ratio, 0.5, 2.);
+ airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
+}
+
+void h_ctl_attitude_loop ( void ) {
+ if (!h_ctl_disabled) {
+ // compute_airspeed_ratio();
+ h_ctl_roll_loop();
+ h_ctl_pitch_loop();
+ }
+}
+
+#define H_CTL_REF_DT (1./60.)
+#define KFFA_UPDATE 0.1
+#define KFFD_UPDATE 0.05
+
+inline static void h_ctl_roll_loop( void ) {
+
+ static float cmd_fb = 0.;
+
+ if (pprz_mode == PPRZ_MODE_MANUAL)
+ h_ctl_roll_sum_err = 0;
+
+ // Update reference setpoints for roll
+ h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
+ h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
+ h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint -
h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;
+ // Saturation on references
+ BoundAbs(h_ctl_ref_roll_accel, H_CTL_REF_MAX_P_DOT);
+ if (h_ctl_ref_roll_rate > H_CTL_REF_MAX_P) {
+ h_ctl_ref_roll_rate = H_CTL_REF_MAX_P;
+ if (h_ctl_ref_roll_accel > 0.) h_ctl_ref_roll_accel = 0.;
+ }
+ else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {
+ h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;
+ if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;
+ }
+
+#ifdef USE_KFF_UPDATE
+ // update Kff gains
+ h_ctl_roll_Kffa -= KFFA_UPDATE * h_ctl_ref_roll_accel * cmd_fb /
(H_CTL_REF_MAX_P_DOT*H_CTL_REF_MAX_P_DOT);
+ h_ctl_roll_Kffd -= KFFD_UPDATE * h_ctl_ref_roll_rate * cmd_fb /
(H_CTL_REF_MAX_P*H_CTL_REF_MAX_P);
+#ifdef SITL
+ printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
+#endif
+ h_ctl_roll_Kffa = Min(h_ctl_roll_Kffa, 0);
+ h_ctl_roll_Kffd = Min(h_ctl_roll_Kffd, 0);
+#endif
+
+ // Compute errors
+ float err = estimator_phi - h_ctl_ref_roll_angle;
+#ifdef SITL
+ static float last_err = 0;
+ estimator_p = (err - last_err)/(1/60.);
+ last_err = err;
+#endif
+ float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT;
+
+ h_ctl_roll_sum_err += err * H_CTL_REF_DT;
+ BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
+
+ cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr;
+ float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
+ + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
+ - cmd_fb
+ - h_ctl_roll_rate_gain * d_err
+ - h_ctl_roll_igain * h_ctl_roll_sum_err
+ + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
+ //float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel
+ // + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
+ // - h_ctl_roll_attitude_gain * err
+ // - h_ctl_roll_rate_gain * derr
+ // - h_ctl_roll_igain * h_ctl_roll_sum_err
+ // + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
+
+ //x cmd /= airspeed_ratio2;
+
+ // Set aileron commands
+ h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
+}
+
+
+// NOT USED
+#ifdef LOITER_TRIM
+float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
+float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
+#endif
+
+#ifdef PITCH_TRIM
+float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
+float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
+
+inline static void loiter(void) {
+ float pitch_trim;
+
+ float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ -
v_ctl_auto_throttle_nominal_cruise_throttle;
+ if (throttle_diff > 0) {
+ float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
+ pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
+ } else {
+ float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle -
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
+ pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
+ }
+
+ h_ctl_pitch_loop_setpoint += pitch_trim;
+}
+#endif
+
+
+inline static void h_ctl_pitch_loop( void ) {
+ static float last_err;
+ /* sanity check */
+ if (h_ctl_pitch_of_roll <0.)
+ h_ctl_pitch_of_roll = 0.;
+
+ if (pprz_mode == PPRZ_MODE_MANUAL)
+ h_ctl_pitch_sum_err = 0;
+
+ h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll *
fabs(estimator_phi);
+#ifdef PITCH_TRIM
+ loiter();
+#endif
+
+ // Update reference setpoints for pitch
+ h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint
- h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
+ h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
+ h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
+ // Saturation on references
+ BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT);
+ if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) {
+ h_ctl_ref_pitch_rate = H_CTL_REF_MAX_Q;
+ if (h_ctl_ref_pitch_accel > 0.) h_ctl_ref_pitch_accel = 0.;
+ }
+ else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) {
+ h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q;
+ if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.;
+ }
+
+ // Compute errors
+ float err = estimator_theta - h_ctl_ref_pitch_angle;
+ float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
+ last_err = err;
+
+ h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
+ BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
+
+ float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
+ + h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate
+ + h_ctl_pitch_pgain * err
+ + h_ctl_pitch_dgain * d_err
+ + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
+
+ // cmd /= airspeed_ratio2;
+
+ h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
+}
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,72 @@
+/*
+ * Paparazzi $Id: fw_h_ctl.h 3784 2009-07-24 14:55:54Z poine $
+ *
+ * Copyright (C) 2009 ENAC
+ *
+ * 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 adaptive control
+ *
+ */
+
+#ifndef FW_H_CTL_A_H
+#define FW_H_CTL_A_H
+
+#include <inttypes.h>
+#include "std.h"
+#include "paparazzi.h"
+#include "airframe.h"
+
+extern float h_ctl_roll_sum_err;
+extern float h_ctl_pitch_sum_err;
+extern float h_ctl_roll_igain;
+extern float h_ctl_pitch_igain;
+extern float h_ctl_roll_Kffa;
+extern float h_ctl_roll_Kffd;
+extern float h_ctl_pitch_Kffa;
+extern float h_ctl_pitch_Kffd;
+extern float h_ctl_pitch_of_roll;
+
+#define H_CTL_ROLL_SUM_ERR_MAX 100.
+#define H_CTL_PITCH_SUM_ERR_MAX 100.
+
+#define fw_h_ctl_a_SetRollIGain(_gain) { \
+ h_ctl_roll_sum_err = 0; \
+ h_ctl_roll_igain = _gain; \
+ }
+
+#define fw_h_ctl_a_SetPitchIGain(_gain) { \
+ h_ctl_pitch_sum_err = 0; \
+ h_ctl_pitch_igain = _gain; \
+ }
+
+/* inner roll loop parameters */
+extern float h_ctl_ref_roll_angle;
+extern float h_ctl_ref_roll_rate;
+extern float h_ctl_ref_roll_accel;
+
+/* inner pitch loop parameters */
+extern float h_ctl_ref_pitch_angle;
+extern float h_ctl_ref_pitch_rate;
+extern float h_ctl_ref_pitch_accel;
+
+#endif /* FW_H_CTL_A_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_h_ctl.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,430 @@
+/*
+ * 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 "firmwares/fixedwing/stabilization.h"
+#include "std.h"
+#include "led.h"
+#include "estimator.h"
+#include "nav.h"
+#include "airframe.h"
+#include "firmwares/fixedwing/guidance.h"
+#include "firmwares/fixedwing/autopilot.h"
+
+
+/* outer loop parameters */
+float h_ctl_course_setpoint; /* rad, CW/north */
+float h_ctl_course_pre_bank;
+float h_ctl_course_pre_bank_correction;
+float h_ctl_course_pgain;
+float h_ctl_course_dgain;
+float h_ctl_roll_max_setpoint;
+
+/* roll and pitch disabling */
+bool_t h_ctl_disabled;
+
+/* AUTO1 rate mode */
+bool_t h_ctl_auto1_rate;
+
+
+/* inner roll loop parameters */
+float h_ctl_roll_setpoint;
+float h_ctl_roll_pgain;
+pprz_t h_ctl_aileron_setpoint;
+float h_ctl_roll_slew;
+
+/* inner pitch loop parameters */
+float h_ctl_pitch_setpoint;
+float h_ctl_pitch_loop_setpoint;
+float h_ctl_pitch_pgain;
+float h_ctl_pitch_dgain;
+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_setpoint;
+float h_ctl_roll_rate_mode;
+float h_ctl_roll_rate_setpoint_pgain;
+float h_ctl_hi_throttle_roll_rate_pgain;
+float h_ctl_lo_throttle_roll_rate_pgain;
+float h_ctl_roll_rate_igain;
+float h_ctl_roll_rate_dgain;
+#endif
+
+#ifdef H_CTL_COURSE_SLEW_INCREMENT
+float h_ctl_course_slew_increment;
+#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( void );
+#endif
+
+#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
+#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
+#endif
+
+#ifndef H_CTL_COURSE_DGAIN
+#define H_CTL_COURSE_DGAIN 0.
+#endif
+
+#ifndef H_CTL_ROLL_RATE_GAIN
+#define H_CTL_ROLL_RATE_GAIN 0.
+#endif
+
+float h_ctl_roll_attitude_gain;
+float h_ctl_roll_rate_gain;
+
+#ifdef AGR_CLIMB
+static float nav_ratio;
+#endif
+
+void h_ctl_init( void ) {
+ h_ctl_course_setpoint = 0.;
+ h_ctl_course_pre_bank = 0.;
+ h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
+ h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
+ h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
+ h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
+
+ h_ctl_disabled = FALSE;
+
+ h_ctl_roll_setpoint = 0.;
+#ifdef H_CTL_ROLL_PGAIN
+ h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
+#endif
+ h_ctl_aileron_setpoint = 0;
+#ifdef H_CTL_AILERON_OF_THROTTLE
+ h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
+#endif
+
+ h_ctl_pitch_setpoint = 0.;
+ h_ctl_pitch_loop_setpoint = 0.;
+ h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
+ h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
+ 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_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
+ h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
+ h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
+ h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
+#endif
+
+#ifdef H_CTL_ROLL_SLEW
+ h_ctl_roll_slew = H_CTL_ROLL_SLEW;
+#endif
+
+#ifdef H_CTL_COURSE_SLEW_INCREMENT
+ h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
+#endif
+
+#ifdef H_CTL_ROLL_ATTITUDE_GAIN
+ h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
+ h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
+#endif
+
+#ifdef AGR_CLIMB
+nav_ratio=0;
+#endif
+}
+
+/**
+ * \brief
+ *
+ */
+void h_ctl_course_loop ( void ) {
+ static float last_err;
+
+ // Ground path error
+ float err = estimator_hspeed_dir - h_ctl_course_setpoint;
+ NormRadAngle(err);
+
+#ifdef STRONG_WIND
+ // Usefull path speed
+ const float reference_advance = (NOMINAL_AIRSPEED / 2.);
+ float advance = cos(err) * estimator_hspeed_mod / reference_advance;
+
+ if (
+ (advance < 1.) && // Path speed is small
+ (estimator_hspeed_mod < reference_advance) // Small path speed is due
to wind (small groundspeed)
+ )
+ {
+/*
+ // rough crabangle approximation
+ float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
+ float wind_dir = atan2(wind_east,wind_north);
+
+ float wind_course = h_ctl_course_setpoint - wind_dir;
+ NormRadAngle(wind_course);
+
+ estimator_hspeed_dir = estimator_psi;
+
+ float crab = sin(wind_dir-estimator_psi) *
atan2(wind_mod,NOMINAL_AIRSPEED);
+ //crab = estimator_hspeed_mod - estimator_psi;
+ NormRadAngle(crab);
+*/
+
+ // Heading error
+ float herr = estimator_psi - h_ctl_course_setpoint; //+crab);
+ NormRadAngle(herr);
+
+ if (advance < -0.5) //<! moving in the wrong direction / big
> 90 degree turn
+ {
+ err = herr;
+ }
+ else if (advance < 0.) //<!
+ {
+ err = (-advance)*2. * herr;
+ }
+ else
+ {
+ err = advance * err;
+ }
+
+ // Reset differentiator when switching mode
+ //if (h_ctl_course_heading_mode == 0)
+ // last_err = err;
+ //h_ctl_course_heading_mode = 1;
+ }
+/* else
+ {
+ // Reset differentiator when switching mode
+ if (h_ctl_course_heading_mode == 1)
+ last_err = err;
+ h_ctl_course_heading_mode = 0;
+ }
+*/
+#endif
+
+ float d_err = err - last_err;
+ last_err = err;
+
+ NormRadAngle(d_err);
+
+#ifdef H_CTL_COURSE_SLEW_INCREMENT
+ /* slew severe course changes (i.e. waypoint moves, block changes or
perpendicular routes) */
+ static float h_ctl_course_slew_rate = 0.;
+ float nav_angle_saturation = -(h_ctl_roll_max_setpoint/h_ctl_course_pgain);
/* heading error corresponding to max_roll */
+ float half_nav_angle_saturation = nav_angle_saturation / 2.;
+ if (launch) { /* prevent accumulator run-up on the ground */
+ if (err > half_nav_angle_saturation) {
+ h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
+ err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
+ h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
+ }
+ else if ( err < -half_nav_angle_saturation) {
+ h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
+ err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
+ h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
+ }
+ else {
+ h_ctl_course_slew_rate = 0.;
+ }
+ }
+#endif
+
+ float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+ Bound(speed_depend_nav, 0.66, 1.5);
+
+ float cmd = h_ctl_course_pgain * speed_depend_nav * (err + d_err *
h_ctl_course_dgain);
+
+
+
+#if defined(AGR_CLIMB) && !defined(USE_AIRSPEED)
+ /** limit navigation during extreme altitude changes */
+ if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent
divide by zero, reversed or negative values */
+ if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
V_CTL_AUTO_THROTTLE_BLENDED) {
+ BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO
and again after */
+ if (v_ctl_altitude_error < 0) {
+ nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 -
(fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START -
AGR_BLEND_END));
+ Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
+ } else {
+ nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 -
(fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START -
AGR_BLEND_END));
+ Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
+ }
+ cmd *= nav_ratio;
+ }
+ }
+#endif
+
+ float roll_setpoint = cmd + h_ctl_course_pre_bank_correction *
h_ctl_course_pre_bank;
+
+#ifdef H_CTL_ROLL_SLEW
+ float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
+ BoundAbs(diff_roll, h_ctl_roll_slew);
+ h_ctl_roll_setpoint += diff_roll;
+#else
+ h_ctl_roll_setpoint = roll_setpoint;
+#endif
+
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
+}
+
+void h_ctl_attitude_loop ( void ) {
+ if (!h_ctl_disabled) {
+ h_ctl_roll_loop();
+ h_ctl_pitch_loop();
+ }
+}
+
+
+#ifdef H_CTL_ROLL_ATTITUDE_GAIN
+inline static void h_ctl_roll_loop( void ) {
+ float err = estimator_phi - h_ctl_roll_setpoint;
+#ifdef SITL
+ static float last_err = 0;
+ estimator_p = (err - last_err)/(1/60.);
+ last_err = err;
+#endif
+ float cmd = - h_ctl_roll_attitude_gain * err
+ - h_ctl_roll_rate_gain * estimator_p
+ + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
+
+ h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
+}
+
+#else // H_CTL_ROLL_ATTITUDE_GAIN
+
+/** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
+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
+ if (h_ctl_auto1_rate) {
+ /** Runs only the roll rate loop */
+ h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
+ h_ctl_roll_rate_loop();
+ } else {
+ h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
+ BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
+
+ float saved_aileron_setpoint = h_ctl_aileron_setpoint;
+ h_ctl_roll_rate_loop();
+ h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint,
saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
+ }
+#endif
+}
+
+#ifdef H_CTL_RATE_LOOP
+
+static inline void h_ctl_roll_rate_loop() {
+ float err = estimator_p - h_ctl_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 throttle_dep_pgain =
+ Blend(h_ctl_hi_throttle_roll_rate_pgain,
h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ));
+ float cmd = throttle_dep_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);
+
+ h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
+}
+#endif /* H_CTL_RATE_LOOP */
+
+#endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
+
+
+
+
+
+
+#ifdef LOITER_TRIM
+
+float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
+float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
+
+inline static float loiter(void) {
+ static float last_elevator_trim;
+ float elevator_trim;
+
+ float throttle_dif = v_ctl_auto_throttle_cruise_throttle -
v_ctl_auto_throttle_nominal_cruise_throttle;
+ if (throttle_dif > 0) {
+ float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
+ elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
+ } else {
+ float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle -
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
+ elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
+ }
+
+ float max_change = (v_ctl_auto_throttle_loiter_trim -
v_ctl_auto_throttle_dash_trim) / 80.;
+ Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim +
max_change);
+
+ last_elevator_trim = elevator_trim;
+ return elevator_trim;
+}
+#endif
+
+
+inline static void h_ctl_pitch_loop( void ) {
+ static float last_err;
+ /* sanity check */
+ if (h_ctl_elevator_of_roll <0.)
+ h_ctl_elevator_of_roll = 0.;
+
+ h_ctl_pitch_loop_setpoint =
+ h_ctl_pitch_setpoint
+ - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
+
+ float err = estimator_theta - h_ctl_pitch_loop_setpoint;
+ float d_err = err - last_err;
+ last_err = err;
+ float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
+#ifdef LOITER_TRIM
+ cmd += loiter();
+#endif
+ h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
+}
+
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
(from rev 6352, paparazzi3/trunk/sw/airborne/fw_h_ctl.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
2010-11-05 00:40:55 UTC (rev 6354)
@@ -0,0 +1,91 @@
+/*
+ * 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 <inttypes.h>
+#include "std.h"
+#include "paparazzi.h"
+#include "airframe.h"
+
+/* outer loop parameters */
+extern float h_ctl_course_setpoint; /* rad, CW/north */
+extern float h_ctl_course_pre_bank;
+extern float h_ctl_course_pre_bank_correction;
+extern float h_ctl_course_pgain;
+extern float h_ctl_course_dgain;
+extern float h_ctl_roll_max_setpoint;
+
+/* roll and pitch disabling */
+extern bool_t h_ctl_disabled;
+
+/* AUTO1 rate mode */
+extern bool_t h_ctl_auto1_rate;
+
+/* inner roll loop parameters */
+extern float h_ctl_roll_setpoint;
+extern float h_ctl_roll_pgain;
+extern pprz_t h_ctl_aileron_setpoint;
+extern float h_ctl_roll_slew;
+
+/* inner pitch loop parameters */
+extern float h_ctl_pitch_setpoint;
+extern float h_ctl_pitch_loop_setpoint;
+extern float h_ctl_pitch_pgain;
+extern float h_ctl_pitch_dgain;
+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_hi_throttle_roll_rate_pgain;
+extern float h_ctl_lo_throttle_roll_rate_pgain;
+extern float h_ctl_roll_rate_igain;
+extern float h_ctl_roll_rate_dgain;
+
+#define fw_h_ctl_SetRollRatePGain(v) { h_ctl_hi_throttle_roll_rate_pgain = v;
h_ctl_lo_throttle_roll_rate_pgain = v; }
+#endif
+
+extern void h_ctl_init( void );
+extern void h_ctl_course_loop ( void );
+extern void h_ctl_attitude_loop ( void );
+
+extern float h_ctl_roll_attitude_gain;
+extern float h_ctl_roll_rate_gain;
+
+#endif /* FW_H_CTL_H */
Deleted: paparazzi3/trunk/sw/airborne/fw_h_ctl.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl.c 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl.c 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,430 +0,0 @@
-/*
- * 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 "std.h"
-#include "led.h"
-#include "fw_h_ctl.h"
-#include "estimator.h"
-#include "nav.h"
-#include "airframe.h"
-#include "fw_v_ctl.h"
-#include "firmwares/fixedwing/autopilot.h"
-
-
-/* outer loop parameters */
-float h_ctl_course_setpoint; /* rad, CW/north */
-float h_ctl_course_pre_bank;
-float h_ctl_course_pre_bank_correction;
-float h_ctl_course_pgain;
-float h_ctl_course_dgain;
-float h_ctl_roll_max_setpoint;
-
-/* roll and pitch disabling */
-bool_t h_ctl_disabled;
-
-/* AUTO1 rate mode */
-bool_t h_ctl_auto1_rate;
-
-
-/* inner roll loop parameters */
-float h_ctl_roll_setpoint;
-float h_ctl_roll_pgain;
-pprz_t h_ctl_aileron_setpoint;
-float h_ctl_roll_slew;
-
-/* inner pitch loop parameters */
-float h_ctl_pitch_setpoint;
-float h_ctl_pitch_loop_setpoint;
-float h_ctl_pitch_pgain;
-float h_ctl_pitch_dgain;
-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_setpoint;
-float h_ctl_roll_rate_mode;
-float h_ctl_roll_rate_setpoint_pgain;
-float h_ctl_hi_throttle_roll_rate_pgain;
-float h_ctl_lo_throttle_roll_rate_pgain;
-float h_ctl_roll_rate_igain;
-float h_ctl_roll_rate_dgain;
-#endif
-
-#ifdef H_CTL_COURSE_SLEW_INCREMENT
-float h_ctl_course_slew_increment;
-#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( void );
-#endif
-
-#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
-#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
-#endif
-
-#ifndef H_CTL_COURSE_DGAIN
-#define H_CTL_COURSE_DGAIN 0.
-#endif
-
-#ifndef H_CTL_ROLL_RATE_GAIN
-#define H_CTL_ROLL_RATE_GAIN 0.
-#endif
-
-float h_ctl_roll_attitude_gain;
-float h_ctl_roll_rate_gain;
-
-#ifdef AGR_CLIMB
-static float nav_ratio;
-#endif
-
-void h_ctl_init( void ) {
- h_ctl_course_setpoint = 0.;
- h_ctl_course_pre_bank = 0.;
- h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
- h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
- h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
- h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
-
- h_ctl_disabled = FALSE;
-
- h_ctl_roll_setpoint = 0.;
-#ifdef H_CTL_ROLL_PGAIN
- h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
-#endif
- h_ctl_aileron_setpoint = 0;
-#ifdef H_CTL_AILERON_OF_THROTTLE
- h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
-#endif
-
- h_ctl_pitch_setpoint = 0.;
- h_ctl_pitch_loop_setpoint = 0.;
- h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
- h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
- 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_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
- h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
- h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
- h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
-#endif
-
-#ifdef H_CTL_ROLL_SLEW
- h_ctl_roll_slew = H_CTL_ROLL_SLEW;
-#endif
-
-#ifdef H_CTL_COURSE_SLEW_INCREMENT
- h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
-#endif
-
-#ifdef H_CTL_ROLL_ATTITUDE_GAIN
- h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
- h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
-#endif
-
-#ifdef AGR_CLIMB
-nav_ratio=0;
-#endif
-}
-
-/**
- * \brief
- *
- */
-void h_ctl_course_loop ( void ) {
- static float last_err;
-
- // Ground path error
- float err = estimator_hspeed_dir - h_ctl_course_setpoint;
- NormRadAngle(err);
-
-#ifdef STRONG_WIND
- // Usefull path speed
- const float reference_advance = (NOMINAL_AIRSPEED / 2.);
- float advance = cos(err) * estimator_hspeed_mod / reference_advance;
-
- if (
- (advance < 1.) && // Path speed is small
- (estimator_hspeed_mod < reference_advance) // Small path speed is due
to wind (small groundspeed)
- )
- {
-/*
- // rough crabangle approximation
- float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
- float wind_dir = atan2(wind_east,wind_north);
-
- float wind_course = h_ctl_course_setpoint - wind_dir;
- NormRadAngle(wind_course);
-
- estimator_hspeed_dir = estimator_psi;
-
- float crab = sin(wind_dir-estimator_psi) *
atan2(wind_mod,NOMINAL_AIRSPEED);
- //crab = estimator_hspeed_mod - estimator_psi;
- NormRadAngle(crab);
-*/
-
- // Heading error
- float herr = estimator_psi - h_ctl_course_setpoint; //+crab);
- NormRadAngle(herr);
-
- if (advance < -0.5) //<! moving in the wrong direction / big
> 90 degree turn
- {
- err = herr;
- }
- else if (advance < 0.) //<!
- {
- err = (-advance)*2. * herr;
- }
- else
- {
- err = advance * err;
- }
-
- // Reset differentiator when switching mode
- //if (h_ctl_course_heading_mode == 0)
- // last_err = err;
- //h_ctl_course_heading_mode = 1;
- }
-/* else
- {
- // Reset differentiator when switching mode
- if (h_ctl_course_heading_mode == 1)
- last_err = err;
- h_ctl_course_heading_mode = 0;
- }
-*/
-#endif
-
- float d_err = err - last_err;
- last_err = err;
-
- NormRadAngle(d_err);
-
-#ifdef H_CTL_COURSE_SLEW_INCREMENT
- /* slew severe course changes (i.e. waypoint moves, block changes or
perpendicular routes) */
- static float h_ctl_course_slew_rate = 0.;
- float nav_angle_saturation = -(h_ctl_roll_max_setpoint/h_ctl_course_pgain);
/* heading error corresponding to max_roll */
- float half_nav_angle_saturation = nav_angle_saturation / 2.;
- if (launch) { /* prevent accumulator run-up on the ground */
- if (err > half_nav_angle_saturation) {
- h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
- err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
- h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
- }
- else if ( err < -half_nav_angle_saturation) {
- h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
- err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
- h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
- }
- else {
- h_ctl_course_slew_rate = 0.;
- }
- }
-#endif
-
- float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
- Bound(speed_depend_nav, 0.66, 1.5);
-
- float cmd = h_ctl_course_pgain * speed_depend_nav * (err + d_err *
h_ctl_course_dgain);
-
-
-
-#if defined(AGR_CLIMB) && !defined(USE_AIRSPEED)
- /** limit navigation during extreme altitude changes */
- if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent
divide by zero, reversed or negative values */
- if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
V_CTL_AUTO_THROTTLE_BLENDED) {
- BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO
and again after */
- if (v_ctl_altitude_error < 0) {
- nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 -
(fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START -
AGR_BLEND_END));
- Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
- } else {
- nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 -
(fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START -
AGR_BLEND_END));
- Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
- }
- cmd *= nav_ratio;
- }
- }
-#endif
-
- float roll_setpoint = cmd + h_ctl_course_pre_bank_correction *
h_ctl_course_pre_bank;
-
-#ifdef H_CTL_ROLL_SLEW
- float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
- BoundAbs(diff_roll, h_ctl_roll_slew);
- h_ctl_roll_setpoint += diff_roll;
-#else
- h_ctl_roll_setpoint = roll_setpoint;
-#endif
-
- BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
-}
-
-void h_ctl_attitude_loop ( void ) {
- if (!h_ctl_disabled) {
- h_ctl_roll_loop();
- h_ctl_pitch_loop();
- }
-}
-
-
-#ifdef H_CTL_ROLL_ATTITUDE_GAIN
-inline static void h_ctl_roll_loop( void ) {
- float err = estimator_phi - h_ctl_roll_setpoint;
-#ifdef SITL
- static float last_err = 0;
- estimator_p = (err - last_err)/(1/60.);
- last_err = err;
-#endif
- float cmd = - h_ctl_roll_attitude_gain * err
- - h_ctl_roll_rate_gain * estimator_p
- + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
-
- h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
-}
-
-#else // H_CTL_ROLL_ATTITUDE_GAIN
-
-/** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
-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
- if (h_ctl_auto1_rate) {
- /** Runs only the roll rate loop */
- h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
- h_ctl_roll_rate_loop();
- } else {
- h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
- BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
-
- float saved_aileron_setpoint = h_ctl_aileron_setpoint;
- h_ctl_roll_rate_loop();
- h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint,
saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
- }
-#endif
-}
-
-#ifdef H_CTL_RATE_LOOP
-
-static inline void h_ctl_roll_rate_loop() {
- float err = estimator_p - h_ctl_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 throttle_dep_pgain =
- Blend(h_ctl_hi_throttle_roll_rate_pgain,
h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ));
- float cmd = throttle_dep_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);
-
- h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
-}
-#endif /* H_CTL_RATE_LOOP */
-
-#endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
-
-
-
-
-
-
-#ifdef LOITER_TRIM
-
-float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
-float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
-
-inline static float loiter(void) {
- static float last_elevator_trim;
- float elevator_trim;
-
- float throttle_dif = v_ctl_auto_throttle_cruise_throttle -
v_ctl_auto_throttle_nominal_cruise_throttle;
- if (throttle_dif > 0) {
- float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
- elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
- } else {
- float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle -
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
- elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
- }
-
- float max_change = (v_ctl_auto_throttle_loiter_trim -
v_ctl_auto_throttle_dash_trim) / 80.;
- Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim +
max_change);
-
- last_elevator_trim = elevator_trim;
- return elevator_trim;
-}
-#endif
-
-
-inline static void h_ctl_pitch_loop( void ) {
- static float last_err;
- /* sanity check */
- if (h_ctl_elevator_of_roll <0.)
- h_ctl_elevator_of_roll = 0.;
-
- h_ctl_pitch_loop_setpoint =
- h_ctl_pitch_setpoint
- - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
-
- float err = estimator_theta - h_ctl_pitch_loop_setpoint;
- float d_err = err - last_err;
- last_err = err;
- float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
-#ifdef LOITER_TRIM
- cmd += loiter();
-#endif
- h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
-}
-
-
Deleted: paparazzi3/trunk/sw/airborne/fw_h_ctl.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,91 +0,0 @@
-/*
- * 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 <inttypes.h>
-#include "std.h"
-#include "paparazzi.h"
-#include "airframe.h"
-
-/* outer loop parameters */
-extern float h_ctl_course_setpoint; /* rad, CW/north */
-extern float h_ctl_course_pre_bank;
-extern float h_ctl_course_pre_bank_correction;
-extern float h_ctl_course_pgain;
-extern float h_ctl_course_dgain;
-extern float h_ctl_roll_max_setpoint;
-
-/* roll and pitch disabling */
-extern bool_t h_ctl_disabled;
-
-/* AUTO1 rate mode */
-extern bool_t h_ctl_auto1_rate;
-
-/* inner roll loop parameters */
-extern float h_ctl_roll_setpoint;
-extern float h_ctl_roll_pgain;
-extern pprz_t h_ctl_aileron_setpoint;
-extern float h_ctl_roll_slew;
-
-/* inner pitch loop parameters */
-extern float h_ctl_pitch_setpoint;
-extern float h_ctl_pitch_loop_setpoint;
-extern float h_ctl_pitch_pgain;
-extern float h_ctl_pitch_dgain;
-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_hi_throttle_roll_rate_pgain;
-extern float h_ctl_lo_throttle_roll_rate_pgain;
-extern float h_ctl_roll_rate_igain;
-extern float h_ctl_roll_rate_dgain;
-
-#define fw_h_ctl_SetRollRatePGain(v) { h_ctl_hi_throttle_roll_rate_pgain = v;
h_ctl_lo_throttle_roll_rate_pgain = v; }
-#endif
-
-extern void h_ctl_init( void );
-extern void h_ctl_course_loop ( void );
-extern void h_ctl_attitude_loop ( void );
-
-extern float h_ctl_roll_attitude_gain;
-extern float h_ctl_roll_rate_gain;
-
-#endif /* FW_H_CTL_H */
Deleted: paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,345 +0,0 @@
-/*
- * Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $
- *
- * Copyright (C) 2009-2010 ENAC
- *
- * 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 adaptive control
- *
- */
-
-#include "std.h"
-#include "led.h"
-#include "fw_h_ctl.h"
-#include "fw_h_ctl_a.h"
-#include "estimator.h"
-#include "nav.h"
-#include "airframe.h"
-#include "fw_v_ctl.h"
-#include "autopilot.h"
-
-
-/* outer loop parameters */
-float h_ctl_course_setpoint; /* rad, CW/north */
-float h_ctl_course_pre_bank;
-float h_ctl_course_pre_bank_correction;
-float h_ctl_course_pgain;
-float h_ctl_course_dgain;
-float h_ctl_roll_max_setpoint;
-
-/* roll and pitch disabling */
-bool_t h_ctl_disabled;
-
-/* AUTO1 rate mode */
-bool_t h_ctl_auto1_rate;
-
-
-float h_ctl_ref_roll_angle;
-float h_ctl_ref_roll_rate;
-float h_ctl_ref_roll_accel;
-float h_ctl_ref_pitch_angle;
-float h_ctl_ref_pitch_rate;
-float h_ctl_ref_pitch_accel;
-#define H_CTL_REF_W 2.5
-#define H_CTL_REF_XI 0.85
-#define H_CTL_REF_MAX_P RadOfDeg(100.)
-#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
-#define H_CTL_REF_MAX_Q RadOfDeg(100.)
-#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
-
-/* inner roll loop parameters */
-float h_ctl_roll_setpoint;
-float h_ctl_roll_attitude_gain;
-float h_ctl_roll_rate_gain;
-float h_ctl_roll_igain;
-float h_ctl_roll_sum_err;
-float h_ctl_roll_Kffa;
-float h_ctl_roll_Kffd;
-pprz_t h_ctl_aileron_setpoint;
-float h_ctl_roll_slew;
-
-float h_ctl_roll_pgain;
-
-/* inner pitch loop parameters */
-float h_ctl_pitch_setpoint;
-float h_ctl_pitch_loop_setpoint;
-float h_ctl_pitch_pgain;
-float h_ctl_pitch_dgain;
-float h_ctl_pitch_igain;
-float h_ctl_pitch_sum_err;
-float h_ctl_pitch_Kffa;
-float h_ctl_pitch_Kffd;
-pprz_t h_ctl_elevator_setpoint;
-
-/* inner loop pre-command */
-float h_ctl_aileron_of_throttle;
-float h_ctl_elevator_of_roll;
-float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
-
-
-inline static void h_ctl_roll_loop( void );
-inline static void h_ctl_pitch_loop( void );
-
-#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
-#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
-#endif
-
-#ifndef H_CTL_COURSE_DGAIN
-#define H_CTL_COURSE_DGAIN 0.
-#endif
-
-#ifndef H_CTL_ROLL_RATE_GAIN
-#define H_CTL_ROLL_RATE_GAIN 0.
-#endif
-
-void h_ctl_init( void ) {
- h_ctl_course_setpoint = 0.;
- h_ctl_course_pre_bank = 0.;
- h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION;
- h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
- h_ctl_course_dgain = H_CTL_COURSE_DGAIN;
- h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
-
- h_ctl_disabled = FALSE;
-
- h_ctl_roll_setpoint = 0.;
- h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
- h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
- h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
- h_ctl_roll_sum_err = 0;
- h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
- h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
- h_ctl_aileron_setpoint = 0;
-#ifdef H_CTL_AILERON_OF_THROTTLE
- h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
-#endif
-
- h_ctl_pitch_setpoint = 0.;
- h_ctl_pitch_loop_setpoint = 0.;
- h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
- h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
- h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
- h_ctl_pitch_sum_err = 0.;
- h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
- h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
- h_ctl_elevator_setpoint = 0;
- h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
-#ifdef H_CTL_PITCH_OF_ROLL
- h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
-#endif
-
-}
-
-/**
- * \brief
- *
- */
-void h_ctl_course_loop ( void ) {
- static float last_err;
-
- // Ground path error
- float err = estimator_hspeed_dir - h_ctl_course_setpoint;
- NormRadAngle(err);
-
- float d_err = err - last_err;
- last_err = err;
- NormRadAngle(d_err);
-
- float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
- Bound(speed_depend_nav, 0.66, 1.5);
-
- h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction *
h_ctl_course_pre_bank
- + h_ctl_course_pgain * speed_depend_nav * err
- + h_ctl_course_dgain * d_err;
-
- BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
-}
-
-static float airspeed_ratio2;
-
-static inline void compute_airspeed_ratio( void ) {
- float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ -
v_ctl_auto_throttle_nominal_cruise_throttle;
- float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */
- if (throttle_diff > 0)
- airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED -
NOMINAL_AIRSPEED);
- else
- airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle
- V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED -
MINIMUM_AIRSPEED);
-
- float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
- Bound(airspeed_ratio, 0.5, 2.);
- airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
-}
-
-void h_ctl_attitude_loop ( void ) {
- if (!h_ctl_disabled) {
- // compute_airspeed_ratio();
- h_ctl_roll_loop();
- h_ctl_pitch_loop();
- }
-}
-
-#define H_CTL_REF_DT (1./60.)
-#define KFFA_UPDATE 0.1
-#define KFFD_UPDATE 0.05
-
-inline static void h_ctl_roll_loop( void ) {
-
- static float cmd_fb = 0.;
-
- if (pprz_mode == PPRZ_MODE_MANUAL)
- h_ctl_roll_sum_err = 0;
-
- // Update reference setpoints for roll
- h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
- h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
- h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint -
h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;
- // Saturation on references
- BoundAbs(h_ctl_ref_roll_accel, H_CTL_REF_MAX_P_DOT);
- if (h_ctl_ref_roll_rate > H_CTL_REF_MAX_P) {
- h_ctl_ref_roll_rate = H_CTL_REF_MAX_P;
- if (h_ctl_ref_roll_accel > 0.) h_ctl_ref_roll_accel = 0.;
- }
- else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {
- h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;
- if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;
- }
-
-#ifdef USE_KFF_UPDATE
- // update Kff gains
- h_ctl_roll_Kffa -= KFFA_UPDATE * h_ctl_ref_roll_accel * cmd_fb /
(H_CTL_REF_MAX_P_DOT*H_CTL_REF_MAX_P_DOT);
- h_ctl_roll_Kffd -= KFFD_UPDATE * h_ctl_ref_roll_rate * cmd_fb /
(H_CTL_REF_MAX_P*H_CTL_REF_MAX_P);
-#ifdef SITL
- printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
-#endif
- h_ctl_roll_Kffa = Min(h_ctl_roll_Kffa, 0);
- h_ctl_roll_Kffd = Min(h_ctl_roll_Kffd, 0);
-#endif
-
- // Compute errors
- float err = estimator_phi - h_ctl_ref_roll_angle;
-#ifdef SITL
- static float last_err = 0;
- estimator_p = (err - last_err)/(1/60.);
- last_err = err;
-#endif
- float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT;
-
- h_ctl_roll_sum_err += err * H_CTL_REF_DT;
- BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
-
- cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr;
- float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
- + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- - cmd_fb
- - h_ctl_roll_rate_gain * d_err
- - h_ctl_roll_igain * h_ctl_roll_sum_err
- + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
- //float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel
- // + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- // - h_ctl_roll_attitude_gain * err
- // - h_ctl_roll_rate_gain * derr
- // - h_ctl_roll_igain * h_ctl_roll_sum_err
- // + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
-
- //x cmd /= airspeed_ratio2;
-
- // Set aileron commands
- h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
-}
-
-
-// NOT USED
-#ifdef LOITER_TRIM
-float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
-float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
-#endif
-
-#ifdef PITCH_TRIM
-float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
-float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
-
-inline static void loiter(void) {
- float pitch_trim;
-
- float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ -
v_ctl_auto_throttle_nominal_cruise_throttle;
- if (throttle_diff > 0) {
- float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE -
v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
- pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
- } else {
- float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle -
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
- pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
- }
-
- h_ctl_pitch_loop_setpoint += pitch_trim;
-}
-#endif
-
-
-inline static void h_ctl_pitch_loop( void ) {
- static float last_err;
- /* sanity check */
- if (h_ctl_pitch_of_roll <0.)
- h_ctl_pitch_of_roll = 0.;
-
- if (pprz_mode == PPRZ_MODE_MANUAL)
- h_ctl_pitch_sum_err = 0;
-
- h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll *
fabs(estimator_phi);
-#ifdef PITCH_TRIM
- loiter();
-#endif
-
- // Update reference setpoints for pitch
- h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint
- h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
- h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
- h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
- // Saturation on references
- BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT);
- if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) {
- h_ctl_ref_pitch_rate = H_CTL_REF_MAX_Q;
- if (h_ctl_ref_pitch_accel > 0.) h_ctl_ref_pitch_accel = 0.;
- }
- else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) {
- h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q;
- if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.;
- }
-
- // Compute errors
- float err = estimator_theta - h_ctl_ref_pitch_angle;
- float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
- last_err = err;
-
- h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
- BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
-
- float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
- + h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate
- + h_ctl_pitch_pgain * err
- + h_ctl_pitch_dgain * d_err
- + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
-
- // cmd /= airspeed_ratio2;
-
- h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
-}
-
Deleted: paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,72 +0,0 @@
-/*
- * Paparazzi $Id: fw_h_ctl.h 3784 2009-07-24 14:55:54Z poine $
- *
- * Copyright (C) 2009 ENAC
- *
- * 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 adaptive control
- *
- */
-
-#ifndef FW_H_CTL_A_H
-#define FW_H_CTL_A_H
-
-#include <inttypes.h>
-#include "std.h"
-#include "paparazzi.h"
-#include "airframe.h"
-
-extern float h_ctl_roll_sum_err;
-extern float h_ctl_pitch_sum_err;
-extern float h_ctl_roll_igain;
-extern float h_ctl_pitch_igain;
-extern float h_ctl_roll_Kffa;
-extern float h_ctl_roll_Kffd;
-extern float h_ctl_pitch_Kffa;
-extern float h_ctl_pitch_Kffd;
-extern float h_ctl_pitch_of_roll;
-
-#define H_CTL_ROLL_SUM_ERR_MAX 100.
-#define H_CTL_PITCH_SUM_ERR_MAX 100.
-
-#define fw_h_ctl_a_SetRollIGain(_gain) { \
- h_ctl_roll_sum_err = 0; \
- h_ctl_roll_igain = _gain; \
- }
-
-#define fw_h_ctl_a_SetPitchIGain(_gain) { \
- h_ctl_pitch_sum_err = 0; \
- h_ctl_pitch_igain = _gain; \
- }
-
-/* inner roll loop parameters */
-extern float h_ctl_ref_roll_angle;
-extern float h_ctl_ref_roll_rate;
-extern float h_ctl_ref_roll_accel;
-
-/* inner pitch loop parameters */
-extern float h_ctl_ref_pitch_angle;
-extern float h_ctl_ref_pitch_rate;
-extern float h_ctl_ref_pitch_accel;
-
-#endif /* FW_H_CTL_A_H */
Deleted: paparazzi3/trunk/sw/airborne/fw_v_ctl.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,126 +0,0 @@
-/*
- * 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 <inttypes.h>
-#include "paparazzi.h"
-
-/* Vertical mode */
-#define V_CTL_MODE_MANUAL 0
-#define V_CTL_MODE_AUTO_THROTTLE 1
-#define V_CTL_MODE_AUTO_CLIMB 2
-#define V_CTL_MODE_AUTO_ALT 3
-#define V_CTL_MODE_NB 4
-extern uint8_t v_ctl_mode;
-
-/* outer loop */
-extern float v_ctl_altitude_error;
-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
-
-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
-
-/* "auto throttle" inner loop parameters */
-extern float v_ctl_auto_throttle_nominal_cruise_throttle;
-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_dgain;
-extern float v_ctl_auto_throttle_sum_err;
-extern float v_ctl_auto_throttle_pitch_of_vz_pgain;
-extern float v_ctl_auto_throttle_pitch_of_vz_dgain;
-
-#ifdef LOITER_TRIM
-extern float v_ctl_auto_throttle_loiter_trim;
-extern float v_ctl_auto_throttle_dash_trim;
-#endif
-
-/* agressive tuning */
-#ifdef TUNE_AGRESSIVE_CLIMB
-extern float agr_climb_throttle;
-extern float agr_climb_pitch;
-extern float agr_climb_nav_ratio;
-extern float agr_descent_throttle;
-extern float agr_descent_pitch;
-extern float agr_descent_nav_ratio;
-#endif
-
-/* "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 pprz_t v_ctl_throttle_slewed;
-
-extern void v_ctl_init( void );
-extern void v_ctl_altitude_loop( void );
-extern void v_ctl_climb_loop ( void );
-
-#ifdef USE_AIRSPEED
-/* "airspeed" inner loop parameters */
-extern float v_ctl_auto_airspeed_setpoint;
-extern float v_ctl_auto_airspeed_controlled;
-extern float v_ctl_auto_airspeed_pgain;
-extern float v_ctl_auto_airspeed_igain;
-extern float v_ctl_auto_airspeed_sum_err;
-extern float v_ctl_auto_groundspeed_setpoint;
-extern float v_ctl_auto_groundspeed_pgain;
-extern float v_ctl_auto_groundspeed_igain;
-extern float v_ctl_auto_groundspeed_sum_err;
-#endif
-
-/** Computes throttle_slewed from throttle_setpoint */
-extern void v_ctl_throttle_slew( void );
-
-#define fw_v_ctl_SetCruiseThrottle(_v) { \
- v_ctl_auto_throttle_cruise_throttle = (_v ? _v :
v_ctl_auto_throttle_nominal_cruise_throttle); \
- Bound(v_ctl_auto_throttle_cruise_throttle,
V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE,
V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); \
-}
-
-#define fw_v_ctl_SetAutoThrottleIgain(_v) { \
- v_ctl_auto_throttle_igain = _v; \
- v_ctl_auto_throttle_sum_err = 0; \
- }
-
-#endif /* FW_V_CTL_H */
Deleted: paparazzi3/trunk/sw/airborne/fw_v_ctl_n.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl_n.c 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl_n.c 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,292 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2010 ENAC
- *
- * 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_n
- * \brief Vertical control for fixed wing vehicles.
- *
- */
-
-#include "fw_v_ctl.h"
-#include "fw_v_ctl_n.h"
-#include "estimator.h"
-#include "nav.h"
-#include "airframe.h"
-#include "autopilot.h"
-
-/* mode */
-uint8_t v_ctl_mode;
-
-/* 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_nominal_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_dgain;
-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;
-float v_ctl_auto_throttle_pitch_of_vz_dgain;
-
-#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
-#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
-#endif
-
-/* "auto pitch" inner loop parameters */
-float v_ctl_auto_pitch_pgain;
-float v_ctl_auto_pitch_dgain;
-float v_ctl_auto_pitch_igain;
-float v_ctl_auto_pitch_sum_err;
-#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
-
-float controlled_throttle;
-pprz_t v_ctl_throttle_setpoint;
-pprz_t v_ctl_throttle_slewed;
-
-// Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
-#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
-
-uint8_t v_ctl_speed_mode;
-
-#ifdef USE_AIRSPEED
-float v_ctl_auto_airspeed_setpoint;
-float v_ctl_auto_airspeed_controlled;
-float v_ctl_auto_airspeed_pgain;
-float v_ctl_auto_airspeed_igain;
-float v_ctl_auto_airspeed_sum_err;
-float v_ctl_auto_groundspeed_setpoint;
-float v_ctl_auto_groundspeed_pgain;
-float v_ctl_auto_groundspeed_igain;
-float v_ctl_auto_groundspeed_sum_err;
-#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
-#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
-#endif
-
-
-void v_ctl_init( void ) {
- /* mode */
- v_ctl_mode = V_CTL_MODE_MANUAL;
- v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
-
- /* 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;
- v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
-
- /* "auto throttle" inner loop parameters */
- v_ctl_auto_throttle_nominal_cruise_throttle =
V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
- v_ctl_auto_throttle_cruise_throttle =
v_ctl_auto_throttle_nominal_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_dgain = 0.;
- v_ctl_auto_throttle_sum_err = 0.;
- v_ctl_auto_throttle_pitch_of_vz_pgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
- v_ctl_auto_throttle_pitch_of_vz_dgain =
V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN;
-
- /* "auto pitch" inner loop parameters */
- v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
- v_ctl_auto_pitch_dgain = V_CTL_AUTO_PITCH_DGAIN;
- v_ctl_auto_pitch_igain = V_CTL_AUTO_PITCH_IGAIN;
- v_ctl_auto_pitch_sum_err = 0.;
-
-#ifdef USE_AIRSPEED
- v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
- v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
- v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
- v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
- v_ctl_auto_airspeed_sum_err = 0.;
-
- v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
- v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
- v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
- v_ctl_auto_groundspeed_sum_err = 0.;
-#endif
-
- controlled_throttle = 0.;
- v_ctl_throttle_setpoint = 0;
-}
-
-/**
- * outer loop
- * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
- */
-
-// Don't use lead controller unless you know what you're doing
-#define LEAD_CTRL_TAU 0.8
-#define LEAD_CTRL_A 1.
-#define LEAD_CTRL_Te (1./60.)
-
-void v_ctl_altitude_loop( void ) {
- static float v_ctl_climb_setpoint_last = 0.;
- //static float last_lead_input = 0.;
-
- // Altitude error
- 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;
-
- // Lead controller
- //v_ctl_climb_setpoint = ((LEAD_CTRL_TAU*LEAD_CTRL_A +
LEAD_CTRL_Te)*climb_sp + LEAD_CTRL_TAU*(v_ctl_climb_setpoint -
LEAD_CTRL_A*last_lead_input)) / (LEAD_CTRL_TAU + LEAD_CTRL_Te);
- //last_lead_input = pitch_sp;
-
- // Limit rate of change of climb setpoint
- float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
- BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT);
- v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
-
- // Limit climb setpoint
- BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
- v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
-}
-
-static inline void v_ctl_set_pitch ( void ) {
- static float last_err = 0.;
-
- // Compute errors
- float err = estimator_z_dot - v_ctl_climb_setpoint;
- float d_err = err - last_err;
- last_err = err;
-
- v_ctl_auto_pitch_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
-
- // PI loop + feedforward ctl
- nav_pitch = nav_pitch
- + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint
- + v_ctl_auto_pitch_pgain * err
- + v_ctl_auto_pitch_dgain * d_err
- + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err;
-
-}
-
-static inline void v_ctl_set_throttle( void ) {
- static float last_err = 0.;
-
- // Compute errors
- float err = estimator_z_dot - v_ctl_climb_setpoint;
- float d_err = err - last_err;
- last_err = err;
-
- v_ctl_auto_throttle_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
-
- // PID loop + feedforward ctl
- 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_dgain * d_err
- + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err;
-
-}
-
-#ifdef USE_AIRSPEED
-static inline void v_ctl_set_airspeed( void ) {
- // Bound airspeed setpoint
- Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
-
- // Airspeed control loop (input: airspeed controlled, output: throttle
controlled)
- float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
- v_ctl_auto_airspeed_sum_err += err_airspeed;
- BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
- controlled_throttle = v_ctl_auto_throttle_cruise_throttle
- + v_ctl_auto_airspeed_pgain * err_airspeed
- + v_ctl_auto_airspeed_igain * v_ctl_auto_airspeed_sum_err;
-
-}
-
-static inline void v_ctl_set_groundspeed( void ) {
- // Ground speed control loop (input: groundspeed error, output: airspeed
controlled)
- float err_groundspeed = v_ctl_auto_groundspeed_setpoint -
estimator_hspeed_mod;
- v_ctl_auto_groundspeed_sum_err += err_groundspeed;
- BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
- v_ctl_auto_airspeed_setpoint = err_groundspeed *
v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err *
v_ctl_auto_groundspeed_igain;
-
-}
-#endif
-
-void v_ctl_climb_loop ( void ) {
-
- // Set pitch
- v_ctl_set_pitch();
- Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
-
- // Set throttle
- switch (v_ctl_speed_mode) {
- case V_CTL_SPEED_THROTTLE:
- v_ctl_set_throttle();
- break;
-#ifdef USE_AIRSPEED
- case V_CTL_SPEED_AIRSPEED:
- v_ctl_set_airspeed();
- break;
- case V_CTL_SPEED_GROUNDSPEED:
- v_ctl_set_groundspeed();
- v_ctl_set_airspeed();
- break;
-#endif
- default:
- controlled_throttle = v_ctl_auto_throttle_cruise_throttle; // ???
- break;
- }
-
- // Set Throttle output
- float f_throttle = controlled_throttle;
- v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
-
-}
-
-#ifdef V_CTL_THROTTLE_SLEW_LIMITER
-#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
-#endif
-
-#ifndef V_CTL_THROTTLE_SLEW
-#define V_CTL_THROTTLE_SLEW 1.
-#endif
-/** \brief Computes slewed throttle from throttle setpoint
- called at 20Hz
- */
-void v_ctl_throttle_slew( void ) {
- pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
- BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
- v_ctl_throttle_slewed += diff_throttle;
-}
-
Deleted: paparazzi3/trunk/sw/airborne/fw_v_ctl_n.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl_n.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl_n.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -1,47 +0,0 @@
-/*
- * $Id: $
- *
- * Copyright (C) 2010 ENAC
- *
- * 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_N_H
-#define FW_V_CTL_N_H
-
-#define V_CTL_SPEED_THROTTLE 0
-#define V_CTL_SPEED_AIRSPEED 1
-#define V_CTL_SPEED_GROUNDSPEED 2
-
-extern float v_ctl_auto_pitch_dgain;
-
-extern uint8_t v_ctl_speed_mode;
-
-#ifdef PITCH_TRIM
-extern float v_ctl_pitch_loiter_trim;
-extern float v_ctl_pitch_dash_trim;
-#endif
-
-#endif /* FW_V_CTL_N_H */
Modified: paparazzi3/trunk/sw/airborne/joystick.h
===================================================================
--- paparazzi3/trunk/sw/airborne/joystick.h 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/joystick.h 2010-11-05 00:40:55 UTC (rev
6354)
@@ -2,7 +2,7 @@
#define JOYSTICK_H
#include "std.h"
-#include "fw_h_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
extern uint8_t joystick_block;
Modified: paparazzi3/trunk/sw/airborne/modules/multi/formation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/formation.c 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/sw/airborne/modules/multi/formation.c 2010-11-05
00:40:55 UTC (rev 6354)
@@ -13,8 +13,8 @@
#include "multi/formation.h"
#include "estimator.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "autopilot.h"
#include "gps.h"
#include "flight_plan.h"
Modified: paparazzi3/trunk/sw/airborne/modules/multi/potential.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/potential.c 2010-11-04
16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/sw/airborne/modules/multi/potential.c 2010-11-05
00:40:55 UTC (rev 6354)
@@ -13,8 +13,8 @@
#include "potential.h"
#include "estimator.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "autopilot.h"
#include "gps.h"
#include "airframe.h"
Modified: paparazzi3/trunk/sw/airborne/nav.c
===================================================================
--- paparazzi3/trunk/sw/airborne/nav.c 2010-11-04 16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/sw/airborne/nav.c 2010-11-05 00:40:55 UTC (rev 6354)
@@ -33,8 +33,8 @@
#include "nav.h"
#include "gps.h"
#include "estimator.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
+#include "firmwares/fixedwing/guidance.h"
#include "firmwares/fixedwing/autopilot.h"
#include "inter_mcu.h"
#include "traffic_info.h"
Modified: paparazzi3/trunk/sw/airborne/nav.h
===================================================================
--- paparazzi3/trunk/sw/airborne/nav.h 2010-11-04 16:18:38 UTC (rev 6353)
+++ paparazzi3/trunk/sw/airborne/nav.h 2010-11-05 00:40:55 UTC (rev 6354)
@@ -34,7 +34,7 @@
#include "std.h"
#include "paparazzi.h"
-#include "fw_v_ctl.h"
+#include "firmwares/fixedwing/guidance.h"
#include "nav_survey_rectangle.h"
#include "common_nav.h"
Modified: paparazzi3/trunk/sw/airborne/rc_settings.c
===================================================================
--- paparazzi3/trunk/sw/airborne/rc_settings.c 2010-11-04 16:18:38 UTC (rev
6353)
+++ paparazzi3/trunk/sw/airborne/rc_settings.c 2010-11-05 00:40:55 UTC (rev
6354)
@@ -32,7 +32,7 @@
#include "nav.h"
#include "estimator.h"
#include "inter_mcu.h"
-#include "fw_h_ctl.h"
+#include "firmwares/fixedwing/stabilization.h"
#define ParamValInt16(param_init_val, param_travel, cur_pulse, init_pulse) \
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6354] moved fw_[hv]_ctl to firmwares/fixedwing/stabilization and guidance,
antoine drouin <=