[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6320] moving main fixedwing files to firmwares/fixe
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6320] moving main fixedwing files to firmwares/fixedwing |
|
Date: |
Mon, 01 Nov 2010 17:18:23 +0000 |
Revision: 6320
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6320
Author: flixr
Date: 2010-11-01 17:18:22 +0000 (Mon, 01 Nov 2010)
Log Message:
-----------
moving main fixedwing files to firmwares/fixedwing
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/demo_module.xml
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/autopilot.makefile
paparazzi3/trunk/conf/autopilot/twin_mcu.makefile
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/fbw_downlink.h
paparazzi3/trunk/sw/airborne/inter_mcu.h
paparazzi3/trunk/sw/airborne/modules/MPPT/MPPT.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
paparazzi3/trunk/sw/airborne/setup_actuators.c
paparazzi3/trunk/sw/simulator/sim_ac_booz.c
paparazzi3/trunk/sw/simulator/sim_ac_fw.c
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.h
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.c
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/main.c
paparazzi3/trunk/sw/airborne/main_ap.c
paparazzi3/trunk/sw/airborne/main_ap.h
paparazzi3/trunk/sw/airborne/main_fbw.c
paparazzi3/trunk/sw/airborne/main_fbw.h
Modified: paparazzi3/trunk/conf/airframes/demo_module.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/demo_module.xml 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/conf/airframes/demo_module.xml 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -165,7 +165,7 @@
FLASH_MODE=IAP
ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED -DTIME_LED=1
-ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c
main_ap.c main.c
+ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
$(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c $(SRC_FIRMWARE)/main.c
ap.CFLAGS += -DINTER_MCU
ap.srcs += inter_mcu.c
Modified: paparazzi3/trunk/conf/autopilot/sitl.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl.makefile 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/conf/autopilot/sitl.makefile 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -1,6 +1,6 @@
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 main_fbw.c
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 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
Modified: paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-11-01
14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-11-01
17:18:22 UTC (rev 6320)
@@ -26,7 +26,7 @@
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 main_fbw.c main_ap.c
datalink.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 += $(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-01
14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/conf/autopilot/sitl_link_pprz.makefile 2010-11-01
17:18:22 UTC (rev 6320)
@@ -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
main_fbw.c main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c
$(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/sim_uart.c datalink.c
+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
Modified: paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile 2010-11-01
14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/conf/autopilot/sitl_link_xbee.makefile 2010-11-01
17:18:22 UTC (rev 6320)
@@ -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 main_fbw.c
main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c
$(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/sim_uart.c
+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
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/autopilot.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/autopilot.makefile
2010-11-01 14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/autopilot.makefile
2010-11-01 17:18:22 UTC (rev 6320)
@@ -95,7 +95,7 @@
# Main
#
-ns_srcs += $(SRC_FIXEDWING)/main.c
+ns_srcs += $(SRC_FIRMWARE)/main.c
#
# LEDs
@@ -141,7 +141,7 @@
##
fbw_CFLAGS += -DFBW
-fbw_srcs += $(SRC_FIXEDWING)/main_fbw.c
+fbw_srcs += $(SRC_FIRMWARE)/main_fbw.c
fbw_srcs += $(SRC_FIXEDWING)/commands.c
######################################################################
@@ -150,7 +150,7 @@
##
ap_CFLAGS += -DAP
-ap_srcs += $(SRC_FIXEDWING)/main_ap.c
+ap_srcs += $(SRC_FIRMWARE)/main_ap.c
ap_srcs += $(SRC_FIXEDWING)/estimator.c
Modified: paparazzi3/trunk/conf/autopilot/twin_mcu.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/twin_mcu.makefile 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/conf/autopilot/twin_mcu.makefile 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -1,6 +1,6 @@
-ap.srcs += main_ap.c sys_time.c main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c
infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c cam.c spi.c rc_settings.c
latlong.c nav_survey_rectangle.c
+ap.srcs += $(SRC_FIRMWARE)/main_ap.c sys_time.c $(SRC_FIRMWARE)/main.c
inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c
estimator.c cam.c spi.c rc_settings.c latlong.c nav_survey_rectangle.c
ap.CFLAGS += -DMCU_SPI_LINK -DGPS -DUBX -DINFRARED -DRADIO_CONTROL -DINTER_MCU
-DSPI_MASTER -DUSE_SPI -DNAV -DRADIO_CONTROL_SETTINGS
-fbw.srcs += sys_time.c main_fbw.c main.c commands.c radio_control.c
pprz_transport.c downlink.c inter_mcu.c spi.c link_mcu.c
+fbw.srcs += sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main.c
commands.c radio_control.c pprz_transport.c downlink.c inter_mcu.c spi.c
link_mcu.c
fbw.CFLAGS += -DRADIO_CONTROL -DDOWNLINK -DUSE_UART0
-DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DINTER_MCU
-DMCU_SPI_LINK -DUART0_BAUD=B38400 -DSPI_SLAVE -DUSE_SPI
Modified: paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.h 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -42,7 +42,7 @@
#include "fw_v_ctl.h"
#include "infrared.h"
#include "commands.h"
-#include "main_ap.h"
+#include "firmwares/fixedwing/main_ap.h"
#include "ap_downlink.h"
#include "sim_uart.h"
#include "latlong.h"
Modified: paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/sw/airborne/arch/sim/sim_ap.c 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -20,7 +20,7 @@
#include "fw_v_ctl.h"
#include "infrared.h"
#include "commands.h"
-#include "main_ap.h"
+#include "firmwares/fixedwing/main_ap.h"
#include "ap_downlink.h"
#include "sim_uart.h"
#include "latlong.h"
Modified: paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/sw/airborne/arch/sim/sim_jsbsim.c 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -20,7 +20,7 @@
#include "fw_v_ctl.h"
#include "infrared.h"
#include "commands.h"
-#include "main_ap.h"
+#include "firmwares/fixedwing/main_ap.h"
#include "ap_downlink.h"
#include "sim_uart.h"
#include "latlong.h"
Modified: paparazzi3/trunk/sw/airborne/fbw_downlink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fbw_downlink.h 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/fbw_downlink.h 2010-11-01 17:18:22 UTC (rev
6320)
@@ -43,7 +43,7 @@
#include "actuators.h"
#include "uart.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_fbw.h"
#include "subsystems/radio_control.h"
#include "inter_mcu.h"
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main.c (from rev 6319,
paparazzi3/trunk/sw/airborne/main.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main.c 2010-11-01
17:18:22 UTC (rev 6320)
@@ -0,0 +1,68 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file main.c
+ * \brief main loop used both on single and dual MCU configuration */
+
+
+#include "sys_time.h"
+
+#ifdef FBW
+#include "firmwares/fixedwing/main_fbw.h"
+#define Fbw(f) f ## _fbw()
+#else
+#define Fbw(f)
+#endif
+
+#ifdef AP
+#include "firmwares/fixedwing/main_ap.h"
+#define Ap(f) f ## _ap()
+#else
+#define Ap(f)
+#endif
+
+#ifdef STM32
+#include "init_hw.h"
+#endif
+
+int main( void ) {
+#ifdef STM32
+ hw_init();
+ sys_time_init();
+#endif
+ Fbw(init);
+ Ap(init);
+ InitSysTimePeriodic();
+ while (1) {
+ if (sys_time_periodic()) {
+ Fbw(periodic_task);
+ Ap(periodic_task);
+#ifdef STM32
+ LED_PERIODIC();
+#endif
+ }
+ Fbw(event_task);
+ Ap(event_task);
+ }
+ return 0;
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c (from rev
6319, paparazzi3/trunk/sw/airborne/main_ap.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.c 2010-11-01
17:18:22 UTC (rev 6320)
@@ -0,0 +1,664 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003-2010 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.
+ *
+ */
+
+/** \file main_ap.c
+ * \brief AP ( AutoPilot ) process
+ *
+ * This process is reponsible for the collecting the different sensors data,
fusing them to obtain
+ * aircraft attitude and running the different control loops
+ */
+
+#define MODULES_C
+
+#include <math.h>
+
+#include "firmwares/fixedwing/main_ap.h"
+
+#include "interrupt_hw.h"
+#include "init_hw.h"
+#include "adc.h"
+#include "fw_h_ctl.h"
+#include "fw_v_ctl.h"
+#include "gps.h"
+#include "infrared.h"
+#include "gyro.h"
+#include "ap_downlink.h"
+#include "nav.h"
+#include "autopilot.h"
+#include "estimator.h"
+#include "settings.h"
+#include "link_mcu.h"
+#include "sys_time.h"
+#include "flight_plan.h"
+#include "datalink.h"
+#include "xbee.h"
+#ifdef STM32
+#include <stm32/gpio.h>
+#else
+#include "gpio.h"
+#endif
+
+#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
+#include "rc_settings.h"
+#endif
+
+#ifdef LED
+#include "led.h"
+#endif
+
+#if defined USE_I2C0 || USE_I2C1
+#include "i2c.h"
+#endif
+
+#ifdef USE_SPI
+#include "spi.h"
+#endif
+
+#ifdef TRAFFIC_INFO
+#include "traffic_info.h"
+#endif
+
+#ifdef USE_USB_SERIAL
+#include "usb_serial.h"
+#endif
+
+#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
+#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in
airframe file)"
+#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
+#endif
+
+#define LOW_BATTERY_DECIVOLT (CATASTROPHIC_BAT_LEVEL*10)
+
+#include "modules.h"
+
+/** FIXME: should be in rc_settings but required by telemetry (ap_downlink.h)*/
+uint8_t rc_settings_mode = 0;
+
+/** Define minimal speed for takeoff in m/s */
+#define MIN_SPEED_FOR_TAKEOFF 5.
+
+bool_t power_switch;
+uint8_t fatal_error_nb = 0;
+static const uint16_t version = 1;
+
+uint8_t pprz_mode = PPRZ_MODE_AUTO2;
+uint8_t lateral_mode = LATERAL_MODE_MANUAL;
+
+static uint8_t mcu1_status;
+
+#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
+static uint8_t mcu1_ppm_cpt;
+#endif
+
+bool_t kill_throttle = FALSE;
+
+float slider_1_val, slider_2_val;
+
+bool_t launch = FALSE;
+
+uint8_t vsupply; // deciVolt
+static int32_t current; // milliAmpere
+
+float energy; // Fuel consumption (mAh)
+
+bool_t gps_lost = FALSE;
+
+
+#define Min(x, y) (x < y ? x : y)
+#define Max(x, y) (x > y ? x : y)
+
+/** \brief Update paparazzi mode
+ */
+#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
+static inline uint8_t pprz_mode_update( void ) {
+ if ((pprz_mode != PPRZ_MODE_HOME &&
+ pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
+#ifdef UNLOCKED_HOME_MODE
+ || TRUE
+#endif
+ ) {
+ return ModeUpdate(pprz_mode,
PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE], fbw_state->status));
+ } else
+ return FALSE;
+}
+#else // not RADIO_CONTROL
+static inline uint8_t pprz_mode_update( void ) {
+ return FALSE;
+}
+#endif
+
+static inline uint8_t mcu1_status_update( void ) {
+ uint8_t new_status = fbw_state->status;
+ if (mcu1_status != new_status) {
+ bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) !=
(new_status&MASK_FBW_CHANGED));
+ mcu1_status = new_status;
+ return changed;
+ }
+ return FALSE;
+}
+
+
+/** \brief Send back uncontrolled channels
+ */
+static inline void copy_from_to_fbw ( void ) {
+#ifdef SetAutoCommandsFromRC
+ SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
+#elif defined RADIO_YAW && defined COMMAND_YAW
+ ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
+#endif
+}
+
+
+
+/*
+ called at 20Hz.
+ sends a serie of initialisation messages followed by a stream of periodic
ones
+*/
+
+/** Define number of message at initialisation */
+#define INIT_MSG_NB 2
+
+uint8_t ac_ident = AC_ID;
+
+/** \brief Send a serie of initialisation messages followed by a stream of
periodic ones
+ *
+ * Called at 60Hz.
+ */
+static inline void reporting_task( void ) {
+ static uint8_t boot = TRUE;
+
+ /** initialisation phase during boot */
+ if (boot) {
+ DOWNLINK_SEND_BOOT(DefaultChannel, &version);
+ boot = FALSE;
+ }
+ /** then report periodicly */
+ else {
+ PeriodicSendAp(DefaultChannel);
+ }
+}
+
+#ifndef RC_LOST_MODE
+#define RC_LOST_MODE PPRZ_MODE_HOME
+#endif
+
+/** \brief Function to be called when a message from FBW is available */
+static inline void telecommand_task( void ) {
+ uint8_t mode_changed = FALSE;
+ copy_from_to_fbw();
+
+ uint8_t really_lost = bit_is_set(fbw_state->status,
STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode ==
PPRZ_MODE_MANUAL);
+ if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER
&& launch) {
+ if (too_far_from_home) {
+ pprz_mode = PPRZ_MODE_HOME;
+ mode_changed = TRUE;
+ }
+ if (really_lost) {
+ pprz_mode = RC_LOST_MODE;
+ mode_changed = TRUE;
+ }
+ }
+ if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
+ bool_t pprz_mode_changed = pprz_mode_update();
+ mode_changed |= pprz_mode_changed;
+#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
+ bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
+ rc_settings(calib_mode_changed || pprz_mode_changed);
+ mode_changed |= calib_mode_changed;
+#endif
+ }
+ mode_changed |= mcu1_status_update();
+ if ( mode_changed )
+ PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+
+#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
+ /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
+ * \a RADIO_ROLL and \a RADIO_PITCH \n
+ */
+ if (pprz_mode == PPRZ_MODE_AUTO1) {
+ /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
+ h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0.,
-AUTO1_MAX_ROLL);
+
+ /** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
+ h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0.,
AUTO1_MAX_PITCH);
+ } /** Else asynchronously set by \a h_ctl_course_loop() */
+
+ /** In AUTO1, throttle comes from RADIO_THROTTLE
+ In MANUAL, the value is copied to get it in the telemetry */
+ if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
+ v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
+ }
+ /** else asynchronously set by v_ctl_climb_loop(); */
+
+ mcu1_ppm_cpt = fbw_state->ppm_cpt;
+#endif // RADIO_CONTROL
+
+
+ vsupply = fbw_state->vsupply;
+ current = fbw_state->current;
+
+#ifdef RADIO_CONTROL
+ if (!estimator_flight_time) {
+ if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] >
THROTTLE_THRESHOLD_TAKEOFF) {
+ launch = TRUE;
+ }
+ }
+#endif
+}
+
+/** \fn void navigation_task( void )
+ * \brief Compute desired_course
+ */
+static void navigation_task( void ) {
+#if defined FAILSAFE_DELAY_WITHOUT_GPS
+ /** This section is used for the failsafe of GPS */
+ static uint8_t last_pprz_mode;
+
+ /** If aircraft is launched and is in autonomus mode, go into
+ PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
+ if (launch) {
+ if (GpsTimeoutError) {
+ if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
+ last_pprz_mode = pprz_mode;
+ pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
+ PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+ gps_lost = TRUE;
+ }
+ } else /* GPS is ok */ if (gps_lost) {
+ /** If aircraft was in failsafe mode, come back in previous mode */
+ pprz_mode = last_pprz_mode;
+ gps_lost = FALSE;
+
+ PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+ }
+ }
+#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
+
+ common_nav_periodic_task_4Hz();
+ if (pprz_mode == PPRZ_MODE_HOME)
+ nav_home();
+ else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
+ nav_without_gps();
+ else
+ nav_periodic_task();
+
+#ifdef TCAS
+ CallTCAS();
+#endif
+
+#ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in
default 0 mode)
+ SEND_NAVIGATION(DefaultChannel);
+#endif
+
+ SEND_CAM(DefaultChannel);
+
+ /* The nav task computes only nav_altitude. However, we are interested
+ by desired_altitude (= nav_alt+alt_shift) in any case.
+ So we always run the altitude control loop */
+ if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
+ v_ctl_altitude_loop();
+
+ if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
+ || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
+#ifdef H_CTL_RATE_LOOP
+ /* Be sure to be in attitude mode, not roll */
+ h_ctl_auto1_rate = FALSE;
+#endif
+ if (lateral_mode >=LATERAL_MODE_COURSE)
+ h_ctl_course_loop(); /* aka compute nav_desired_roll */
+ if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
+ v_ctl_climb_loop();
+ if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
+ v_ctl_throttle_setpoint = nav_throttle_setpoint;
+
+#if defined V_CTL_THROTTLE_IDLE
+ Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ),
MAX_PPRZ);
+#endif
+
+#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
+ if (vsupply > 0.) {
+ v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL /
(float)vsupply;
+ v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
+ }
+#endif
+
+ h_ctl_pitch_setpoint = nav_pitch;
+ Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT,
H_CTL_PITCH_MAX_SETPOINT);
+ if (kill_throttle || (!estimator_flight_time && !launch))
+ v_ctl_throttle_setpoint = 0;
+ }
+ energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz
-> hours)
+}
+
+
+#ifndef KILL_MODE_DISTANCE
+#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
+#endif
+
+
+/** Maximum time allowed for low battery level */
+#define LOW_BATTERY_DELAY 5
+
+/** \fn inline void periodic_task( void )
+ * \brief Do periodic tasks at 60 Hz
+ */
+/**There are four @@@@@ boucles @@@@@:
+ * - 20 Hz:
+ * - lets use \a reporting_task at 60 Hz
+ * - updates ir with \a ir_update
+ * - updates estimator of ir with \a estimator_update_state_infrared
+ * - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
+ * - sends to \a fbw \a desired_throttle, \a desired_aileron and
+ * \a desired_elevator \note \a desired_throttle is set upon GPS
+ * message reception
+ * - 4 Hz:
+ * - calls \a estimator_propagate_state
+ * - do navigation with \a navigation_task
+ *
+ */
+
+void periodic_task_ap( void ) {
+ static uint8_t _20Hz = 0;
+ static uint8_t _10Hz = 0;
+ static uint8_t _4Hz = 0;
+ static uint8_t _1Hz = 0;
+
+ _20Hz++;
+ if (_20Hz>=3) _20Hz=0;
+ _10Hz++;
+ if (_10Hz>=6) _10Hz=0;
+ _4Hz++;
+ if (_4Hz>=15) _4Hz=0;
+ _1Hz++;
+ if (_1Hz>=60) _1Hz=0;
+
+ reporting_task();
+
+ if (!_1Hz) {
+ if (estimator_flight_time) estimator_flight_time++;
+#if defined DATALINK || defined SITL
+ datalink_time++;
+#endif
+
+ static uint8_t t = 0;
+ if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
+ kill_throttle |= (t >= LOW_BATTERY_DELAY);
+ kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
+ }
+
+ switch(_4Hz) {
+ case 0:
+#ifdef SITL
+#ifdef GPS_TRIGGERED_FUNCTION
+ GPS_TRIGGERED_FUNCTION();
+#endif
+#endif
+ estimator_propagate_state();
+#ifdef EXTRA_DOWNLINK_DEVICE
+
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
+#endif
+ navigation_task();
+ break;
+ case 1:
+ if (!estimator_flight_time &&
+ estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
+ estimator_flight_time = 1;
+ launch = TRUE; /* Not set in non auto launch */
+ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
+ default:
+ break;
+ }
+
+ break;
+
+#ifdef USE_GPIO
+ case 3:
+ GpioUpdate1();
+ break;
+#endif
+
+ /* default: */
+ }
+
+#ifndef CONTROL_RATE
+#define CONTROL_RATE 20
+#endif
+
+#if CONTROL_RATE != 60 && CONTROL_RATE != 20
+#error "Only 20 and 60 allowed for CONTROL_RATE"
+#endif
+
+#if CONTROL_RATE == 20
+ if (!_20Hz)
+#endif
+ {
+
+#if defined GYRO
+ gyro_update();
+#endif
+
+#ifdef INFRARED
+ ir_update();
+ estimator_update_state_infrared();
+#endif /* INFRARED */
+ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint &
h_ctl_elevator_setpoint */
+ v_ctl_throttle_slew();
+ ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
+ ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
+ ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
+
+#if defined MCU_SPI_LINK
+ link_mcu_send();
+#elif defined INTER_MCU && defined SINGLE_MCU
+ /**Directly set the flag indicating to FBW that shared buffer is
available*/
+ inter_mcu_received_ap = TRUE;
+#endif
+
+ }
+
+ modules_periodic_task();
+}
+
+
+void init_ap( void ) {
+#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
+ hw_init();
+ sys_time_init();
+
+#ifdef LED
+ led_init();
+#endif
+
+#ifdef ADC
+ adc_init();
+#endif
+#endif /* SINGLE_MCU */
+
+ /************* Sensors initialization ***************/
+#ifdef INFRARED
+ ir_init();
+#endif
+#ifdef GYRO
+ gyro_init();
+#endif
+#ifdef GPS
+ gps_init();
+#endif
+#ifdef USE_UART0
+ Uart0Init();
+#endif
+#ifdef USE_UART1
+ Uart1Init();
+#endif
+
+#ifdef USE_USB_SERIAL
+ VCOM_init();
+#endif
+
+#ifdef USE_GPIO
+ GpioInit();
+#endif
+
+#ifdef USE_I2C0
+ i2c0_init();
+#endif
+
+#ifdef USE_I2C1
+ i2c1_init();
+#endif
+
+ /************* Links initialization ***************/
+#if defined USE_SPI
+ spi_init();
+#endif
+#if defined MCU_SPI_LINK
+ link_mcu_init();
+#endif
+#ifdef MODEM
+ modem_init();
+#endif
+
+ /************ Internal status ***************/
+ h_ctl_init();
+ v_ctl_init();
+ estimator_init();
+#ifdef ALT_KALMAN
+ alt_kalman_init();
+#endif
+ nav_init();
+
+ modules_init();
+
+ /** - start interrupt task */
+ int_enable();
+
+ /** wait 0.5s (historical :-) */
+#ifndef STM32
+ sys_time_usleep(500000);
+#endif
+
+#if defined GPS_CONFIGURE
+ gps_configure_uart();
+#endif
+
+#if defined DATALINK
+
+#if DATALINK == XBEE
+ xbee_init();
+#endif
+#endif /* DATALINK */
+
+#if defined AEROCOMM_DATA_PIN
+ IO0DIR |= _BV(AEROCOMM_DATA_PIN);
+ IO0SET = _BV(AEROCOMM_DATA_PIN);
+#endif
+
+ power_switch = FALSE;
+
+ /************ Multi-uavs status ***************/
+
+#ifdef TRAFFIC_INFO
+ traffic_info_init();
+#endif
+
+}
+
+
+/*********** EVENT ***********************************************************/
+void event_task_ap( void ) {
+
+#ifdef GPS
+#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the
datalink */
+ if (GpsBuffer()) {
+ ReadGpsBuffer();
+ }
+#endif
+ if (gps_msg_received) {
+ /* parse and use GPS messages */
+#ifdef GPS_CONFIGURE
+ if (gps_configuring)
+ gps_configure();
+ else
+#endif
+ parse_gps_msg();
+ gps_msg_received = FALSE;
+ if (gps_pos_available) {
+ gps_verbose_downlink = !launch;
+ UseGpsPosNoSend(estimator_update_state_gps);
+ gps_downlink();
+#ifdef GPS_TRIGGERED_FUNCTION
+#ifndef SITL
+ GPS_TRIGGERED_FUNCTION();
+#endif
+#endif
+ gps_pos_available = FALSE;
+ }
+ }
+#endif /** GPS */
+
+
+#if defined DATALINK
+
+#if DATALINK == PPRZ
+ if (PprzBuffer()) {
+ ReadPprzBuffer();
+ if (pprz_msg_received) {
+ pprz_parse_payload();
+ pprz_msg_received = FALSE;
+ }
+ }
+#elif DATALINK == XBEE
+ if (XBeeBuffer()) {
+ ReadXBeeBuffer();
+ if (xbee_msg_received) {
+ xbee_parse_payload();
+ xbee_msg_received = FALSE;
+ }
+ }
+#else
+#error "Unknown DATALINK"
+#endif
+
+ if (dl_msg_available) {
+ dl_parse_msg();
+ dl_msg_available = FALSE;
+ }
+#endif /** DATALINK */
+
+#ifdef MCU_SPI_LINK
+ if (spi_message_received) {
+ /* Got a message on SPI. */
+ spi_message_received = FALSE;
+ link_mcu_event_task();
+ }
+#endif
+
+ if (inter_mcu_received_fbw) {
+ /* receive radio control task from fbw */
+ inter_mcu_received_fbw = FALSE;
+ telecommand_task();
+ }
+
+ modules_event_task();
+} /* event_task_ap() */
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.h (from rev
6319, paparazzi3/trunk/sw/airborne/main_ap.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_ap.h 2010-11-01
17:18:22 UTC (rev 6320)
@@ -0,0 +1,37 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file main_ap.h
+ * \brief AP ( AutoPilot ) process API
+ *
+ */
+
+#ifndef AP_H
+#define AP_H
+
+extern void init_ap( void );
+extern void periodic_task_ap( void );
+extern void event_task_ap( void );
+
+#endif
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.c (from rev
6319, paparazzi3/trunk/sw/airborne/main_fbw.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.c 2010-11-01
17:18:22 UTC (rev 6320)
@@ -0,0 +1,239 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2003-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.
+ *
+ */
+
+/** \file main_fbw.c
+ * \brief FBW ( FlyByWire ) process
+ *
+ * This process is responsible for decoding radio control, generating
actuators
+ * signals either from the radio control or from the commands provided by the
+ * AP (autopilot) process. It also performs a telemetry task and a low level
monitoring
+ * ( for parameters like the supply )
+ */
+
+#include "firmwares/fixedwing/main_fbw.h"
+#include "airframe.h"
+
+#include "init_hw.h"
+#include "interrupt_hw.h"
+#include "led.h"
+#include "uart.h"
+#include "spi.h"
+#include "adc.h"
+
+#ifdef USE_USB_SERIAL
+#include "usb_serial.h"
+#endif
+
+#include "sys_time.h"
+#include "commands.h"
+#include "firmwares/fixedwing/actuators.h"
+#include "subsystems/radio_control.h"
+#include "fbw_downlink.h"
+#include "autopilot.h"
+#include "paparazzi.h"
+#include "estimator.h"
+
+#ifdef MCU_SPI_LINK
+#include "link_mcu.h"
+#endif
+
+#ifdef MILLIAMP_PER_PERCENT
+# warning "deprecated MILLIAMP_PER_PERCENT --> Please use
MILLIAMP_AT_FULL_THROTTLE"
+#endif
+
+#ifdef ADC
+struct adc_buf vsupply_adc_buf;
+#ifndef VoltageOfAdc
+#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
+#endif
+#ifdef ADC_CHANNEL_CURRENT
+struct adc_buf current_adc_buf;
+#ifndef MilliAmpereOfAdc
+#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
+#endif
+#endif
+#endif
+
+uint8_t fbw_vsupply_decivolt;
+int32_t fbw_current_milliamp;
+
+uint8_t fbw_mode;
+
+#include "inter_mcu.h"
+
+/********** INIT *************************************************************/
+void init_fbw( void ) {
+ hw_init();
+ sys_time_init();
+#ifdef LED
+ led_init();
+#endif
+#ifdef USE_UART0
+ uart0_init();
+#endif
+#ifdef USE_UART1
+ uart1_init();
+#endif
+#ifdef ADC
+ adc_init();
+ adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
+# ifdef ADC_CHANNEL_CURRENT
+ adc_buf_channel(ADC_CHANNEL_CURRENT, ¤t_adc_buf, DEFAULT_AV_NB_SAMPLE);
+# endif
+#endif
+#ifdef ACTUATORS
+ actuators_init();
+ /* Load the failsafe defaults */
+ SetCommands(commands_failsafe);
+#endif
+#ifdef RADIO_CONTROL
+ radio_control_init();
+#endif
+#ifdef INTER_MCU
+ inter_mcu_init();
+#endif
+#ifdef MCU_SPI_LINK
+ spi_init();
+ link_mcu_restart();
+#endif
+
+ fbw_mode = FBW_MODE_FAILSAFE;
+
+#ifndef SINGLE_MCU
+ int_enable();
+#endif
+}
+
+
+static inline void set_failsafe_mode( void ) {
+ fbw_mode = FBW_MODE_FAILSAFE;
+ SetCommands(commands_failsafe);
+}
+
+#ifdef RADIO_CONTROL
+static inline void handle_rc_frame( void ) {
+ fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
+ if (fbw_mode == FBW_MODE_MANUAL)
+ SetCommandsFromRC(commands, radio_control.values);
+}
+#endif
+
+/********** EVENT ************************************************************/
+
+void event_task_fbw( void) {
+#ifdef RADIO_CONTROL
+ RadioControlEvent(handle_rc_frame);
+#endif
+
+
+#ifdef INTER_MCU
+#ifdef MCU_SPI_LINK
+ if (spi_message_received) {
+ /* Got a message on SPI. */
+ spi_message_received = FALSE;
+
+ /* Sets link_mcu_received */
+ /* Sets inter_mcu_received_ap if checksum is ok */
+ link_mcu_event_task();
+ }
+#endif /* MCU_SPI_LINK */
+
+
+ if (inter_mcu_received_ap) {
+ inter_mcu_received_ap = FALSE;
+ inter_mcu_event_task();
+ if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
+ fbw_mode = FBW_MODE_AUTO;
+ }
+ if (fbw_mode == FBW_MODE_AUTO) {
+ SetCommands(ap_state->commands);
+ }
+#ifdef SetApOnlyCommands
+ else
+ {
+ SetApOnlyCommands(ap_state->commands);
+ }
+#endif
+#ifdef SINGLE_MCU
+ inter_mcu_fill_fbw_state();
+#endif /**Else the buffer is filled even if the last receive was not correct */
+ }
+
+#ifdef MCU_SPI_LINK
+ if (link_mcu_received) {
+ link_mcu_received = FALSE;
+ inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
+ link_mcu_restart(); /** Prepares the next SPI communication */
+ }
+#endif /* MCU_SPI_LINK */
+#endif /* INTER_MCU */
+
+}
+
+
+/************* PERIODIC ******************************************************/
+void periodic_task_fbw( void ) {
+ static uint8_t _10Hz; /* FIXME : sys_time should provide it */
+ _10Hz++;
+ if (_10Hz >= 6) _10Hz = 0;
+
+#ifdef RADIO_CONTROL
+ radio_control_periodic_task();
+ if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
+ fbw_mode = FBW_MODE_AUTO;
+ }
+#endif
+
+#ifdef INTER_MCU
+ inter_mcu_periodic_task();
+ if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
+ set_failsafe_mode();
+#endif
+
+#ifdef DOWNLINK
+ fbw_downlink_periodic_task();
+#endif
+
+ if (!_10Hz)
+ {
+#ifdef ADC
+ fbw_vsupply_decivolt =
VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
+# ifdef ADC_CHANNEL_CURRENT
+ fbw_current_milliamp =
MilliAmpereOfAdc((current_adc_buf.sum/current_adc_buf.av_nb_sample));
+# endif
+#endif
+
+#if ((! defined ADC_CHANNEL_CURRENT) && defined MILLIAMP_AT_FULL_THROTTLE)
+#ifdef COMMAND_THROTTLE
+ fbw_current_milliamp = Min(((float)commands[COMMAND_THROTTLE]) *
((float)MILLIAMP_AT_FULL_THROTTLE) / ((float)MAX_PPRZ), 65000);
+#endif
+# endif
+ }
+
+#ifdef ACTUATORS
+ SetActuatorsFromCommands(commands);
+#endif
+
+
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.h (from rev
6319, paparazzi3/trunk/sw/airborne/main_fbw.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/main_fbw.h 2010-11-01
17:18:22 UTC (rev 6320)
@@ -0,0 +1,51 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file main_fbw.h
+ * \brief FBW ( FlyByWire ) process API
+ *
+ */
+
+#ifndef FBW_H
+#define FBW_H
+
+#include "std.h"
+#include "adc.h"
+
+/** Fly by wire modes */
+#define FBW_MODE_MANUAL 0
+#define FBW_MODE_AUTO 1
+#define FBW_MODE_FAILSAFE 2
+#define FBW_MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ?
FBW_MODE_MANUAL : FBW_MODE_AUTO)
+
+extern uint8_t fbw_mode;
+extern uint8_t fbw_vsupply_decivolt;
+extern int32_t fbw_current_milliamp;
+extern bool_t failsafe_mode;
+
+void init_fbw( void );
+void periodic_task_fbw( void );
+void event_task_fbw( void );
+
+#endif
Modified: paparazzi3/trunk/sw/airborne/inter_mcu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/inter_mcu.h 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/inter_mcu.h 2010-11-01 17:18:22 UTC (rev
6320)
@@ -42,7 +42,7 @@
#include "paparazzi.h"
#include "airframe.h"
#include "subsystems/radio_control.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_fbw.h"
#ifndef SINGLE_MCU
#include "radio.h"
Deleted: paparazzi3/trunk/sw/airborne/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main.c 2010-11-01 14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/sw/airborne/main.c 2010-11-01 17:18:22 UTC (rev 6320)
@@ -1,68 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-/** \file main.c
- * \brief main loop used both on single and dual MCU configuration */
-
-
-#include "sys_time.h"
-
-#ifdef FBW
-#include "main_fbw.h"
-#define Fbw(f) f ## _fbw()
-#else
-#define Fbw(f)
-#endif
-
-#ifdef AP
-#include "main_ap.h"
-#define Ap(f) f ## _ap()
-#else
-#define Ap(f)
-#endif
-
-#ifdef STM32
-#include "init_hw.h"
-#endif
-
-int main( void ) {
-#ifdef STM32
- hw_init();
- sys_time_init();
-#endif
- Fbw(init);
- Ap(init);
- InitSysTimePeriodic();
- while (1) {
- if (sys_time_periodic()) {
- Fbw(periodic_task);
- Ap(periodic_task);
-#ifdef STM32
- LED_PERIODIC();
-#endif
- }
- Fbw(event_task);
- Ap(event_task);
- }
- return 0;
-}
Deleted: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/main_ap.c 2010-11-01 17:18:22 UTC (rev
6320)
@@ -1,664 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003-2010 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.
- *
- */
-
-/** \file main_ap.c
- * \brief AP ( AutoPilot ) process
- *
- * This process is reponsible for the collecting the different sensors data,
fusing them to obtain
- * aircraft attitude and running the different control loops
- */
-
-#define MODULES_C
-
-#include <math.h>
-
-#include "main_ap.h"
-
-#include "interrupt_hw.h"
-#include "init_hw.h"
-#include "adc.h"
-#include "fw_h_ctl.h"
-#include "fw_v_ctl.h"
-#include "gps.h"
-#include "infrared.h"
-#include "gyro.h"
-#include "ap_downlink.h"
-#include "nav.h"
-#include "autopilot.h"
-#include "estimator.h"
-#include "settings.h"
-#include "link_mcu.h"
-#include "sys_time.h"
-#include "flight_plan.h"
-#include "datalink.h"
-#include "xbee.h"
-#ifdef STM32
-#include <stm32/gpio.h>
-#else
-#include "gpio.h"
-#endif
-
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
-#include "rc_settings.h"
-#endif
-
-#ifdef LED
-#include "led.h"
-#endif
-
-#if defined USE_I2C0 || USE_I2C1
-#include "i2c.h"
-#endif
-
-#ifdef USE_SPI
-#include "spi.h"
-#endif
-
-#ifdef TRAFFIC_INFO
-#include "traffic_info.h"
-#endif
-
-#ifdef USE_USB_SERIAL
-#include "usb_serial.h"
-#endif
-
-#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
-#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in
airframe file)"
-#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
-#endif
-
-#define LOW_BATTERY_DECIVOLT (CATASTROPHIC_BAT_LEVEL*10)
-
-#include "modules.h"
-
-/** FIXME: should be in rc_settings but required by telemetry (ap_downlink.h)*/
-uint8_t rc_settings_mode = 0;
-
-/** Define minimal speed for takeoff in m/s */
-#define MIN_SPEED_FOR_TAKEOFF 5.
-
-bool_t power_switch;
-uint8_t fatal_error_nb = 0;
-static const uint16_t version = 1;
-
-uint8_t pprz_mode = PPRZ_MODE_AUTO2;
-uint8_t lateral_mode = LATERAL_MODE_MANUAL;
-
-static uint8_t mcu1_status;
-
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
-static uint8_t mcu1_ppm_cpt;
-#endif
-
-bool_t kill_throttle = FALSE;
-
-float slider_1_val, slider_2_val;
-
-bool_t launch = FALSE;
-
-uint8_t vsupply; // deciVolt
-static int32_t current; // milliAmpere
-
-float energy; // Fuel consumption (mAh)
-
-bool_t gps_lost = FALSE;
-
-
-#define Min(x, y) (x < y ? x : y)
-#define Max(x, y) (x > y ? x : y)
-
-/** \brief Update paparazzi mode
- */
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
-static inline uint8_t pprz_mode_update( void ) {
- if ((pprz_mode != PPRZ_MODE_HOME &&
- pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
-#ifdef UNLOCKED_HOME_MODE
- || TRUE
-#endif
- ) {
- return ModeUpdate(pprz_mode,
PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE], fbw_state->status));
- } else
- return FALSE;
-}
-#else // not RADIO_CONTROL
-static inline uint8_t pprz_mode_update( void ) {
- return FALSE;
-}
-#endif
-
-static inline uint8_t mcu1_status_update( void ) {
- uint8_t new_status = fbw_state->status;
- if (mcu1_status != new_status) {
- bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) !=
(new_status&MASK_FBW_CHANGED));
- mcu1_status = new_status;
- return changed;
- }
- return FALSE;
-}
-
-
-/** \brief Send back uncontrolled channels
- */
-static inline void copy_from_to_fbw ( void ) {
-#ifdef SetAutoCommandsFromRC
- SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
-#elif defined RADIO_YAW && defined COMMAND_YAW
- ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
-#endif
-}
-
-
-
-/*
- called at 20Hz.
- sends a serie of initialisation messages followed by a stream of periodic
ones
-*/
-
-/** Define number of message at initialisation */
-#define INIT_MSG_NB 2
-
-uint8_t ac_ident = AC_ID;
-
-/** \brief Send a serie of initialisation messages followed by a stream of
periodic ones
- *
- * Called at 60Hz.
- */
-static inline void reporting_task( void ) {
- static uint8_t boot = TRUE;
-
- /** initialisation phase during boot */
- if (boot) {
- DOWNLINK_SEND_BOOT(DefaultChannel, &version);
- boot = FALSE;
- }
- /** then report periodicly */
- else {
- PeriodicSendAp(DefaultChannel);
- }
-}
-
-#ifndef RC_LOST_MODE
-#define RC_LOST_MODE PPRZ_MODE_HOME
-#endif
-
-/** \brief Function to be called when a message from FBW is available */
-static inline void telecommand_task( void ) {
- uint8_t mode_changed = FALSE;
- copy_from_to_fbw();
-
- uint8_t really_lost = bit_is_set(fbw_state->status,
STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode ==
PPRZ_MODE_MANUAL);
- if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER
&& launch) {
- if (too_far_from_home) {
- pprz_mode = PPRZ_MODE_HOME;
- mode_changed = TRUE;
- }
- if (really_lost) {
- pprz_mode = RC_LOST_MODE;
- mode_changed = TRUE;
- }
- }
- if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
- bool_t pprz_mode_changed = pprz_mode_update();
- mode_changed |= pprz_mode_changed;
-#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
- bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
- rc_settings(calib_mode_changed || pprz_mode_changed);
- mode_changed |= calib_mode_changed;
-#endif
- }
- mode_changed |= mcu1_status_update();
- if ( mode_changed )
- PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
-
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
- /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
- * \a RADIO_ROLL and \a RADIO_PITCH \n
- */
- if (pprz_mode == PPRZ_MODE_AUTO1) {
- /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
- h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0.,
-AUTO1_MAX_ROLL);
-
- /** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
- h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0.,
AUTO1_MAX_PITCH);
- } /** Else asynchronously set by \a h_ctl_course_loop() */
-
- /** In AUTO1, throttle comes from RADIO_THROTTLE
- In MANUAL, the value is copied to get it in the telemetry */
- if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
- v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
- }
- /** else asynchronously set by v_ctl_climb_loop(); */
-
- mcu1_ppm_cpt = fbw_state->ppm_cpt;
-#endif // RADIO_CONTROL
-
-
- vsupply = fbw_state->vsupply;
- current = fbw_state->current;
-
-#ifdef RADIO_CONTROL
- if (!estimator_flight_time) {
- if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] >
THROTTLE_THRESHOLD_TAKEOFF) {
- launch = TRUE;
- }
- }
-#endif
-}
-
-/** \fn void navigation_task( void )
- * \brief Compute desired_course
- */
-static void navigation_task( void ) {
-#if defined FAILSAFE_DELAY_WITHOUT_GPS
- /** This section is used for the failsafe of GPS */
- static uint8_t last_pprz_mode;
-
- /** If aircraft is launched and is in autonomus mode, go into
- PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
- if (launch) {
- if (GpsTimeoutError) {
- if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
- last_pprz_mode = pprz_mode;
- pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
- PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
- gps_lost = TRUE;
- }
- } else /* GPS is ok */ if (gps_lost) {
- /** If aircraft was in failsafe mode, come back in previous mode */
- pprz_mode = last_pprz_mode;
- gps_lost = FALSE;
-
- PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
- }
- }
-#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
-
- common_nav_periodic_task_4Hz();
- if (pprz_mode == PPRZ_MODE_HOME)
- nav_home();
- else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
- nav_without_gps();
- else
- nav_periodic_task();
-
-#ifdef TCAS
- CallTCAS();
-#endif
-
-#ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in
default 0 mode)
- SEND_NAVIGATION(DefaultChannel);
-#endif
-
- SEND_CAM(DefaultChannel);
-
- /* The nav task computes only nav_altitude. However, we are interested
- by desired_altitude (= nav_alt+alt_shift) in any case.
- So we always run the altitude control loop */
- if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
- v_ctl_altitude_loop();
-
- if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
- || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
-#ifdef H_CTL_RATE_LOOP
- /* Be sure to be in attitude mode, not roll */
- h_ctl_auto1_rate = FALSE;
-#endif
- if (lateral_mode >=LATERAL_MODE_COURSE)
- h_ctl_course_loop(); /* aka compute nav_desired_roll */
- if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
- v_ctl_climb_loop();
- if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
- v_ctl_throttle_setpoint = nav_throttle_setpoint;
-
-#if defined V_CTL_THROTTLE_IDLE
- Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ),
MAX_PPRZ);
-#endif
-
-#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
- if (vsupply > 0.) {
- v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL /
(float)vsupply;
- v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
- }
-#endif
-
- h_ctl_pitch_setpoint = nav_pitch;
- Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT,
H_CTL_PITCH_MAX_SETPOINT);
- if (kill_throttle || (!estimator_flight_time && !launch))
- v_ctl_throttle_setpoint = 0;
- }
- energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz
-> hours)
-}
-
-
-#ifndef KILL_MODE_DISTANCE
-#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
-#endif
-
-
-/** Maximum time allowed for low battery level */
-#define LOW_BATTERY_DELAY 5
-
-/** \fn inline void periodic_task( void )
- * \brief Do periodic tasks at 60 Hz
- */
-/**There are four @@@@@ boucles @@@@@:
- * - 20 Hz:
- * - lets use \a reporting_task at 60 Hz
- * - updates ir with \a ir_update
- * - updates estimator of ir with \a estimator_update_state_infrared
- * - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
- * - sends to \a fbw \a desired_throttle, \a desired_aileron and
- * \a desired_elevator \note \a desired_throttle is set upon GPS
- * message reception
- * - 4 Hz:
- * - calls \a estimator_propagate_state
- * - do navigation with \a navigation_task
- *
- */
-
-void periodic_task_ap( void ) {
- static uint8_t _20Hz = 0;
- static uint8_t _10Hz = 0;
- static uint8_t _4Hz = 0;
- static uint8_t _1Hz = 0;
-
- _20Hz++;
- if (_20Hz>=3) _20Hz=0;
- _10Hz++;
- if (_10Hz>=6) _10Hz=0;
- _4Hz++;
- if (_4Hz>=15) _4Hz=0;
- _1Hz++;
- if (_1Hz>=60) _1Hz=0;
-
- reporting_task();
-
- if (!_1Hz) {
- if (estimator_flight_time) estimator_flight_time++;
-#if defined DATALINK || defined SITL
- datalink_time++;
-#endif
-
- static uint8_t t = 0;
- if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
- kill_throttle |= (t >= LOW_BATTERY_DELAY);
- kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
- }
-
- switch(_4Hz) {
- case 0:
-#ifdef SITL
-#ifdef GPS_TRIGGERED_FUNCTION
- GPS_TRIGGERED_FUNCTION();
-#endif
-#endif
- estimator_propagate_state();
-#ifdef EXTRA_DOWNLINK_DEVICE
-
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
-#endif
- navigation_task();
- break;
- case 1:
- if (!estimator_flight_time &&
- estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
- estimator_flight_time = 1;
- launch = TRUE; /* Not set in non auto launch */
- DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
- default:
- break;
- }
-
- break;
-
-#ifdef USE_GPIO
- case 3:
- GpioUpdate1();
- break;
-#endif
-
- /* default: */
- }
-
-#ifndef CONTROL_RATE
-#define CONTROL_RATE 20
-#endif
-
-#if CONTROL_RATE != 60 && CONTROL_RATE != 20
-#error "Only 20 and 60 allowed for CONTROL_RATE"
-#endif
-
-#if CONTROL_RATE == 20
- if (!_20Hz)
-#endif
- {
-
-#if defined GYRO
- gyro_update();
-#endif
-
-#ifdef INFRARED
- ir_update();
- estimator_update_state_infrared();
-#endif /* INFRARED */
- h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint &
h_ctl_elevator_setpoint */
- v_ctl_throttle_slew();
- ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
- ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
- ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
-
-#if defined MCU_SPI_LINK
- link_mcu_send();
-#elif defined INTER_MCU && defined SINGLE_MCU
- /**Directly set the flag indicating to FBW that shared buffer is
available*/
- inter_mcu_received_ap = TRUE;
-#endif
-
- }
-
- modules_periodic_task();
-}
-
-
-void init_ap( void ) {
-#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
- hw_init();
- sys_time_init();
-
-#ifdef LED
- led_init();
-#endif
-
-#ifdef ADC
- adc_init();
-#endif
-#endif /* SINGLE_MCU */
-
- /************* Sensors initialization ***************/
-#ifdef INFRARED
- ir_init();
-#endif
-#ifdef GYRO
- gyro_init();
-#endif
-#ifdef GPS
- gps_init();
-#endif
-#ifdef USE_UART0
- Uart0Init();
-#endif
-#ifdef USE_UART1
- Uart1Init();
-#endif
-
-#ifdef USE_USB_SERIAL
- VCOM_init();
-#endif
-
-#ifdef USE_GPIO
- GpioInit();
-#endif
-
-#ifdef USE_I2C0
- i2c0_init();
-#endif
-
-#ifdef USE_I2C1
- i2c1_init();
-#endif
-
- /************* Links initialization ***************/
-#if defined USE_SPI
- spi_init();
-#endif
-#if defined MCU_SPI_LINK
- link_mcu_init();
-#endif
-#ifdef MODEM
- modem_init();
-#endif
-
- /************ Internal status ***************/
- h_ctl_init();
- v_ctl_init();
- estimator_init();
-#ifdef ALT_KALMAN
- alt_kalman_init();
-#endif
- nav_init();
-
- modules_init();
-
- /** - start interrupt task */
- int_enable();
-
- /** wait 0.5s (historical :-) */
-#ifndef STM32
- sys_time_usleep(500000);
-#endif
-
-#if defined GPS_CONFIGURE
- gps_configure_uart();
-#endif
-
-#if defined DATALINK
-
-#if DATALINK == XBEE
- xbee_init();
-#endif
-#endif /* DATALINK */
-
-#if defined AEROCOMM_DATA_PIN
- IO0DIR |= _BV(AEROCOMM_DATA_PIN);
- IO0SET = _BV(AEROCOMM_DATA_PIN);
-#endif
-
- power_switch = FALSE;
-
- /************ Multi-uavs status ***************/
-
-#ifdef TRAFFIC_INFO
- traffic_info_init();
-#endif
-
-}
-
-
-/*********** EVENT ***********************************************************/
-void event_task_ap( void ) {
-
-#ifdef GPS
-#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the
datalink */
- if (GpsBuffer()) {
- ReadGpsBuffer();
- }
-#endif
- if (gps_msg_received) {
- /* parse and use GPS messages */
-#ifdef GPS_CONFIGURE
- if (gps_configuring)
- gps_configure();
- else
-#endif
- parse_gps_msg();
- gps_msg_received = FALSE;
- if (gps_pos_available) {
- gps_verbose_downlink = !launch;
- UseGpsPosNoSend(estimator_update_state_gps);
- gps_downlink();
-#ifdef GPS_TRIGGERED_FUNCTION
-#ifndef SITL
- GPS_TRIGGERED_FUNCTION();
-#endif
-#endif
- gps_pos_available = FALSE;
- }
- }
-#endif /** GPS */
-
-
-#if defined DATALINK
-
-#if DATALINK == PPRZ
- if (PprzBuffer()) {
- ReadPprzBuffer();
- if (pprz_msg_received) {
- pprz_parse_payload();
- pprz_msg_received = FALSE;
- }
- }
-#elif DATALINK == XBEE
- if (XBeeBuffer()) {
- ReadXBeeBuffer();
- if (xbee_msg_received) {
- xbee_parse_payload();
- xbee_msg_received = FALSE;
- }
- }
-#else
-#error "Unknown DATALINK"
-#endif
-
- if (dl_msg_available) {
- dl_parse_msg();
- dl_msg_available = FALSE;
- }
-#endif /** DATALINK */
-
-#ifdef MCU_SPI_LINK
- if (spi_message_received) {
- /* Got a message on SPI. */
- spi_message_received = FALSE;
- link_mcu_event_task();
- }
-#endif
-
- if (inter_mcu_received_fbw) {
- /* receive radio control task from fbw */
- inter_mcu_received_fbw = FALSE;
- telecommand_task();
- }
-
- modules_event_task();
-} /* event_task_ap() */
Deleted: paparazzi3/trunk/sw/airborne/main_ap.h
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.h 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/main_ap.h 2010-11-01 17:18:22 UTC (rev
6320)
@@ -1,37 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file main_ap.h
- * \brief AP ( AutoPilot ) process API
- *
- */
-
-#ifndef AP_H
-#define AP_H
-
-extern void init_ap( void );
-extern void periodic_task_ap( void );
-extern void event_task_ap( void );
-
-#endif
Deleted: paparazzi3/trunk/sw/airborne/main_fbw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_fbw.c 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/main_fbw.c 2010-11-01 17:18:22 UTC (rev
6320)
@@ -1,239 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2003-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.
- *
- */
-
-/** \file main_fbw.c
- * \brief FBW ( FlyByWire ) process
- *
- * This process is responsible for decoding radio control, generating
actuators
- * signals either from the radio control or from the commands provided by the
- * AP (autopilot) process. It also performs a telemetry task and a low level
monitoring
- * ( for parameters like the supply )
- */
-
-#include "main_fbw.h"
-#include "airframe.h"
-
-#include "init_hw.h"
-#include "interrupt_hw.h"
-#include "led.h"
-#include "uart.h"
-#include "spi.h"
-#include "adc.h"
-
-#ifdef USE_USB_SERIAL
-#include "usb_serial.h"
-#endif
-
-#include "sys_time.h"
-#include "commands.h"
-#include "firmwares/fixedwing/actuators.h"
-#include "subsystems/radio_control.h"
-#include "fbw_downlink.h"
-#include "autopilot.h"
-#include "paparazzi.h"
-#include "estimator.h"
-
-#ifdef MCU_SPI_LINK
-#include "link_mcu.h"
-#endif
-
-#ifdef MILLIAMP_PER_PERCENT
-# warning "deprecated MILLIAMP_PER_PERCENT --> Please use
MILLIAMP_AT_FULL_THROTTLE"
-#endif
-
-#ifdef ADC
-struct adc_buf vsupply_adc_buf;
-#ifndef VoltageOfAdc
-#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
-#endif
-#ifdef ADC_CHANNEL_CURRENT
-struct adc_buf current_adc_buf;
-#ifndef MilliAmpereOfAdc
-#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
-#endif
-#endif
-#endif
-
-uint8_t fbw_vsupply_decivolt;
-int32_t fbw_current_milliamp;
-
-uint8_t fbw_mode;
-
-#include "inter_mcu.h"
-
-/********** INIT *************************************************************/
-void init_fbw( void ) {
- hw_init();
- sys_time_init();
-#ifdef LED
- led_init();
-#endif
-#ifdef USE_UART0
- uart0_init();
-#endif
-#ifdef USE_UART1
- uart1_init();
-#endif
-#ifdef ADC
- adc_init();
- adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
-# ifdef ADC_CHANNEL_CURRENT
- adc_buf_channel(ADC_CHANNEL_CURRENT, ¤t_adc_buf, DEFAULT_AV_NB_SAMPLE);
-# endif
-#endif
-#ifdef ACTUATORS
- actuators_init();
- /* Load the failsafe defaults */
- SetCommands(commands_failsafe);
-#endif
-#ifdef RADIO_CONTROL
- radio_control_init();
-#endif
-#ifdef INTER_MCU
- inter_mcu_init();
-#endif
-#ifdef MCU_SPI_LINK
- spi_init();
- link_mcu_restart();
-#endif
-
- fbw_mode = FBW_MODE_FAILSAFE;
-
-#ifndef SINGLE_MCU
- int_enable();
-#endif
-}
-
-
-static inline void set_failsafe_mode( void ) {
- fbw_mode = FBW_MODE_FAILSAFE;
- SetCommands(commands_failsafe);
-}
-
-#ifdef RADIO_CONTROL
-static inline void handle_rc_frame( void ) {
- fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
- if (fbw_mode == FBW_MODE_MANUAL)
- SetCommandsFromRC(commands, radio_control.values);
-}
-#endif
-
-/********** EVENT ************************************************************/
-
-void event_task_fbw( void) {
-#ifdef RADIO_CONTROL
- RadioControlEvent(handle_rc_frame);
-#endif
-
-
-#ifdef INTER_MCU
-#ifdef MCU_SPI_LINK
- if (spi_message_received) {
- /* Got a message on SPI. */
- spi_message_received = FALSE;
-
- /* Sets link_mcu_received */
- /* Sets inter_mcu_received_ap if checksum is ok */
- link_mcu_event_task();
- }
-#endif /* MCU_SPI_LINK */
-
-
- if (inter_mcu_received_ap) {
- inter_mcu_received_ap = FALSE;
- inter_mcu_event_task();
- if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
- fbw_mode = FBW_MODE_AUTO;
- }
- if (fbw_mode == FBW_MODE_AUTO) {
- SetCommands(ap_state->commands);
- }
-#ifdef SetApOnlyCommands
- else
- {
- SetApOnlyCommands(ap_state->commands);
- }
-#endif
-#ifdef SINGLE_MCU
- inter_mcu_fill_fbw_state();
-#endif /**Else the buffer is filled even if the last receive was not correct */
- }
-
-#ifdef MCU_SPI_LINK
- if (link_mcu_received) {
- link_mcu_received = FALSE;
- inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
- link_mcu_restart(); /** Prepares the next SPI communication */
- }
-#endif /* MCU_SPI_LINK */
-#endif /* INTER_MCU */
-
-}
-
-
-/************* PERIODIC ******************************************************/
-void periodic_task_fbw( void ) {
- static uint8_t _10Hz; /* FIXME : sys_time should provide it */
- _10Hz++;
- if (_10Hz >= 6) _10Hz = 0;
-
-#ifdef RADIO_CONTROL
- radio_control_periodic_task();
- if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
- fbw_mode = FBW_MODE_AUTO;
- }
-#endif
-
-#ifdef INTER_MCU
- inter_mcu_periodic_task();
- if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
- set_failsafe_mode();
-#endif
-
-#ifdef DOWNLINK
- fbw_downlink_periodic_task();
-#endif
-
- if (!_10Hz)
- {
-#ifdef ADC
- fbw_vsupply_decivolt =
VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
-# ifdef ADC_CHANNEL_CURRENT
- fbw_current_milliamp =
MilliAmpereOfAdc((current_adc_buf.sum/current_adc_buf.av_nb_sample));
-# endif
-#endif
-
-#if ((! defined ADC_CHANNEL_CURRENT) && defined MILLIAMP_AT_FULL_THROTTLE)
-#ifdef COMMAND_THROTTLE
- fbw_current_milliamp = Min(((float)commands[COMMAND_THROTTLE]) *
((float)MILLIAMP_AT_FULL_THROTTLE) / ((float)MAX_PPRZ), 65000);
-#endif
-# endif
- }
-
-#ifdef ACTUATORS
- SetActuatorsFromCommands(commands);
-#endif
-
-
-}
Deleted: paparazzi3/trunk/sw/airborne/main_fbw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/main_fbw.h 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/airborne/main_fbw.h 2010-11-01 17:18:22 UTC (rev
6320)
@@ -1,51 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file main_fbw.h
- * \brief FBW ( FlyByWire ) process API
- *
- */
-
-#ifndef FBW_H
-#define FBW_H
-
-#include "std.h"
-#include "adc.h"
-
-/** Fly by wire modes */
-#define FBW_MODE_MANUAL 0
-#define FBW_MODE_AUTO 1
-#define FBW_MODE_FAILSAFE 2
-#define FBW_MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ?
FBW_MODE_MANUAL : FBW_MODE_AUTO)
-
-extern uint8_t fbw_mode;
-extern uint8_t fbw_vsupply_decivolt;
-extern int32_t fbw_current_milliamp;
-extern bool_t failsafe_mode;
-
-void init_fbw( void );
-void periodic_task_fbw( void );
-void event_task_fbw( void );
-
-#endif
Modified: paparazzi3/trunk/sw/airborne/modules/MPPT/MPPT.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/MPPT/MPPT.c 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/sw/airborne/modules/MPPT/MPPT.c 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -25,7 +25,7 @@
#include <stdbool.h>
#include "MPPT.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_fbw.h"
#include "i2c.h"
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c 2010-11-01
14:59:19 UTC (rev 6319)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c 2010-11-01
17:18:22 UTC (rev 6320)
@@ -7,7 +7,7 @@
#include <stdbool.h>
#include "ins_arduimu.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_fbw.h"
#include "i2c.h"
// test
Modified: paparazzi3/trunk/sw/airborne/setup_actuators.c
===================================================================
--- paparazzi3/trunk/sw/airborne/setup_actuators.c 2010-11-01 14:59:19 UTC
(rev 6319)
+++ paparazzi3/trunk/sw/airborne/setup_actuators.c 2010-11-01 17:18:22 UTC
(rev 6320)
@@ -10,7 +10,7 @@
#include "datalink.h"
#include "uart.h"
#include "pprz_transport.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_fbw.h"
#include "downlink.h"
#include "settings.h"
Modified: paparazzi3/trunk/sw/simulator/sim_ac_booz.c
===================================================================
--- paparazzi3/trunk/sw/simulator/sim_ac_booz.c 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/simulator/sim_ac_booz.c 2010-11-01 17:18:22 UTC (rev
6320)
@@ -24,8 +24,8 @@
#include <string.h>
#include "sim_ac_jsbsim.h"
-#include "main_ap.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_ap.h"
+#include "firmwares/fixedwing/main_fbw.h"
using namespace JSBSim;
Modified: paparazzi3/trunk/sw/simulator/sim_ac_fw.c
===================================================================
--- paparazzi3/trunk/sw/simulator/sim_ac_fw.c 2010-11-01 14:59:19 UTC (rev
6319)
+++ paparazzi3/trunk/sw/simulator/sim_ac_fw.c 2010-11-01 17:18:22 UTC (rev
6320)
@@ -24,8 +24,8 @@
#include <string.h>
#include "sim_ac_jsbsim.h"
-#include "main_ap.h"
-#include "main_fbw.h"
+#include "firmwares/fixedwing/main_ap.h"
+#include "firmwares/fixedwing/main_fbw.h"
#include "jsbsim_hw.h"
#include <iostream>
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6320] moving main fixedwing files to firmwares/fixedwing,
Felix Ruess <=