[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5963] starting to merge booz, moving booz2_autopilo
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5963] starting to merge booz, moving booz2_autopilot to firmwares/ rotorcraft/autopilot |
|
Date: |
Mon, 27 Sep 2010 22:54:54 +0000 |
Revision: 5963
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5963
Author: flixr
Date: 2010-09-27 22:54:54 +0000 (Mon, 27 Sep 2010)
Log Message:
-----------
starting to merge booz, moving booz2_autopilot to firmwares/rotorcraft/autopilot
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml
paparazzi3/trunk/conf/settings/booz_dc.xml
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/booz_fms.h
paparazzi3/trunk/sw/airborne/csc/mercury_ap.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/autopilot.h
paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -44,7 +44,7 @@
CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/
-BOOZ_INC = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
+BOOZ_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
ap.ARCHDIR = $(ARCH)
@@ -178,7 +178,7 @@
# include subsystems/rotorcraft/ahrs_lkf.makefile
#
-ap.srcs += $(SRC_BOOZ)/booz2_autopilot.c
+ap.srcs += $(SRC_FIRMWARE)/autopilot.c
ap.srcs += math/pprz_trig_int.c
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-27 21:23:52 UTC (rev 5962)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-27 22:54:54 UTC (rev 5963)
@@ -86,7 +86,7 @@
#sim.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/booz_imu_b2.h\"
#sim.CFLAGS += -DIMU_B2_VERSION_1_1
-sim.srcs += $(SRC_BOOZ)/booz2_autopilot.c
+sim.srcs += $(SRC_FIRMWARE)/autopilot.c
#
# in makefile section of airframe xml
Modified: paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml 2010-09-27
21:23:52 UTC (rev 5962)
+++ paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml 2010-09-27
22:54:54 UTC (rev 5963)
@@ -2,7 +2,7 @@
<flight_plan alt="5" ground_alt="0" lat0="37.6136" lon0="-122.3569"
max_dist_from_home="400" name="Booz Test Sim" security_height="2">
<header>
-#include "booz2_autopilot.h"
+#include "autopilot.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
Modified: paparazzi3/trunk/conf/settings/booz_dc.xml
===================================================================
--- paparazzi3/trunk/conf/settings/booz_dc.xml 2010-09-27 21:23:52 UTC (rev
5962)
+++ paparazzi3/trunk/conf/settings/booz_dc.xml 2010-09-27 22:54:54 UTC (rev
5963)
@@ -3,7 +3,7 @@
<settings>
<dl_settings>
<dl_settings NAME="DC">
- <dl_setting MAX="1" MIN="0" STEP="1" VAR="booz2_autopilot_power_switch"
module="booz2_autopilot" handler="SetPowerSwitch" shortname="Shutter">
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="booz2_autopilot_power_switch"
module="autopilot" handler="SetPowerSwitch" shortname="Shutter">
<strip_button name="Photo" icon="digital-camera.png" value="1"/>
<strip_button name="Photo Off" icon="digital-camera-off.png" value="0"/>
</dl_setting>
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -9,13 +9,13 @@
<key_press key="v" value="7"/>
<key_press key="h" value="8"/>
</dl_setting>
- <dl_setting var="booz2_autopilot_mode_auto2" min="0" step="1" max="12"
module="booz2_autopilot" shortname="auto2"
values="Fail|Kill|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav"/>
- <dl_setting var="kill_throttle" min="0" step="1" max="1"
module="booz2_autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
- <dl_setting var="booz2_autopilot_power_switch" min="0" step="1" max="1"
module="booz2_autopilot" values="OFF|ON" handler="SetPowerSwitch">
+ <dl_setting var="booz2_autopilot_mode_auto2" min="0" step="1" max="12"
module="autopilot" shortname="auto2"
values="Fail|Kill|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav"/>
+ <dl_setting var="kill_throttle" min="0" step="1" max="1"
module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
+ <dl_setting var="booz2_autopilot_power_switch" min="0" step="1" max="1"
module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
<strip_button name="POWER ON" icon="on.png" value="1"/>
<strip_button name="POWER OFF" icon="off.png" value="0"/>
</dl_setting>
- <dl_setting var="booz2_autopilot_rc" min="0" step="1" max="1"
module="booz2_autopilot" values="RC OFF|RC ON">
+ <dl_setting var="booz2_autopilot_rc" min="0" step="1" max="1"
module="autopilot" values="RC OFF|RC ON">
<strip_button name="RC ON" value="1"/>
<strip_button name="RC OFF" value="0"/>
</dl_setting>
Deleted: paparazzi3/trunk/sw/airborne/autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/autopilot.h 2010-09-27 21:23:52 UTC (rev
5962)
+++ paparazzi3/trunk/sw/airborne/autopilot.h 2010-09-27 22:54:54 UTC (rev
5963)
@@ -1,124 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file autopilot.h
- * \brief Autopilot modes
- *
- */
-
-#ifndef AUTOPILOT_H
-#define AUTOPILOT_H
-
-#include <inttypes.h>
-#include "std.h"
-#include "sys_time.h"
-
-#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
-
-#define TRESHOLD1 TRESHOLD_MANUAL_PPRZ
-#define TRESHOLD2 (MAX_PPRZ/2)
-
-
-#define PPRZ_MODE_MANUAL 0
-#define PPRZ_MODE_AUTO1 1
-#define PPRZ_MODE_AUTO2 2
-#define PPRZ_MODE_HOME 3
-#define PPRZ_MODE_GPS_OUT_OF_ORDER 4
-#define PPRZ_MODE_NB 5
-
-#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \
- (pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
- (pprz > TRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
-
-extern uint8_t pprz_mode;
-extern bool_t kill_throttle;
-
-
-#define LATERAL_MODE_MANUAL 0
-#define LATERAL_MODE_ROLL_RATE 1
-#define LATERAL_MODE_ROLL 2
-#define LATERAL_MODE_COURSE 3
-#define LATERAL_MODE_NB 4
-
-#define STICK_PUSHED(pprz) (pprz < TRESHOLD1 || pprz > TRESHOLD2)
-
-
-#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ *
travel + center)
-
-extern uint8_t fatal_error_nb;
-
-#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
-
-extern uint8_t lateral_mode;
-extern uint8_t vsupply;
-
-extern float slider_1_val, slider_2_val;
-
-extern bool_t launch;
-
-extern uint8_t light_mode;
-extern bool_t gps_lost;
-
-extern bool_t sum_err_reset;
-
-/** Assignment, returning _old_value != _value
- * Using GCC expression statements */
-#define ModeUpdate(_mode, _value) ({ \
- uint8_t new_mode = _value; \
- (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
-})
-
-void periodic_task( void );
-void telecommand_task(void);
-
-#ifdef RADIO_CONTROL
-#include "radio_control.h"
-static inline void autopilot_process_radio_control ( void ) {
- pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0);
-}
-#endif
-
-extern bool_t power_switch;
-
-#ifdef POWER_SWITCH_LED
-#define autopilot_SetPowerSwitch(_x) { \
- power_switch = _x; \
- if (_x) LED_ON(POWER_SWITCH_LED) else LED_OFF(POWER_SWITCH_LED); \
-}
-#else // POWER_SWITCH_LED
-#define autopilot_SetPowerSwitch(_x) { power_switch = _x; }
-#endif // POWER_SWITCH_LED
-
-#define autopilot_ResetFlightTimeAndLaunch(_) { \
- estimator_flight_time = 0; launch = FALSE; \
-}
-
-
-/* For backward compatibility with old airframe files */
-#include "airframe.h"
-#ifndef CONTROL_RATE
-#define CONTROL_RATE 20
-#endif
-
-#endif /* AUTOPILOT_H */
Modified:
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.c
2010-09-27 21:23:52 UTC (rev 5962)
+++
paparazzi3/trunk/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.c
2010-09-27 22:54:54 UTC (rev 5963)
@@ -30,7 +30,7 @@
#include "uart.h"
#include "booz_radio_control.h"
#include "booz_radio_control_spektrum_arch.h"
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#define SPEKTRUM_CHANNELS_PER_FRAME 7
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -1,259 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * 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.
- *
- */
-
-#include "booz2_autopilot.h"
-
-#include "booz_radio_control.h"
-#include "booz2_commands.h"
-#include "booz2_navigation.h"
-#include "booz_guidance.h"
-#include "booz_stabilization.h"
-#include "led.h"
-
-uint8_t booz2_autopilot_mode;
-uint8_t booz2_autopilot_mode_auto2;
-bool_t booz2_autopilot_motors_on;
-bool_t booz2_autopilot_in_flight;
-uint32_t booz2_autopilot_motors_on_counter;
-uint32_t booz2_autopilot_in_flight_counter;
-bool_t kill_throttle;
-bool_t booz2_autopilot_rc;
-
-bool_t booz2_autopilot_power_switch;
-
-bool_t booz2_autopilot_detect_ground;
-bool_t booz2_autopilot_detect_ground_once;
-
-uint16_t booz2_autopilot_flight_time;
-
-#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40
-#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
-#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
-#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
-
-void booz2_autopilot_init(void) {
- booz2_autopilot_mode = BOOZ2_AP_MODE_KILL;
- booz2_autopilot_motors_on = FALSE;
- booz2_autopilot_in_flight = FALSE;
- kill_throttle = ! booz2_autopilot_motors_on;
- booz2_autopilot_motors_on_counter = 0;
- booz2_autopilot_in_flight_counter = 0;
- booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
- booz2_autopilot_detect_ground = FALSE;
- booz2_autopilot_detect_ground_once = FALSE;
- booz2_autopilot_flight_time = 0;
- booz2_autopilot_rc = TRUE;
- booz2_autopilot_power_switch = FALSE;
- LED_ON(POWER_SWITCH_LED); // POWER OFF
-}
-
-
-void booz2_autopilot_periodic(void) {
-
- RunOnceEvery(BOOZ2_NAV_PRESCALER, nav_periodic_task());
-#ifdef BOOZ_FAILSAFE_GROUND_DETECT
- if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE &&
booz2_autopilot_detect_ground) {
- booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
- booz2_autopilot_detect_ground = FALSE;
- }
-#endif
- if ( !booz2_autopilot_motors_on ||
-#ifndef BOOZ_FAILSAFE_GROUND_DETECT
- booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE ||
-#endif
- booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) {
- SetCommands(booz2_commands_failsafe,
- booz2_autopilot_in_flight, booz2_autopilot_motors_on);
- }
- else {
- booz2_guidance_v_run( booz2_autopilot_in_flight );
- booz2_guidance_h_run( booz2_autopilot_in_flight );
- SetCommands(booz_stabilization_cmd,
- booz2_autopilot_in_flight, booz2_autopilot_motors_on);
- }
-
-}
-
-
-void booz2_autopilot_set_mode(uint8_t new_autopilot_mode) {
-
- if (new_autopilot_mode != booz2_autopilot_mode) {
- /* horizontal mode */
- switch (new_autopilot_mode) {
- case BOOZ2_AP_MODE_FAILSAFE:
-#ifndef BOOZ_KILL_AS_FAILSAFE
- booz_stab_att_sp_euler.phi = 0;
- booz_stab_att_sp_euler.theta = 0;
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
- break;
-#endif
- case BOOZ2_AP_MODE_KILL:
- booz2_autopilot_motors_on = FALSE;
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL);
- break;
- case BOOZ2_AP_MODE_RATE_DIRECT:
- case BOOZ2_AP_MODE_RATE_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE);
- break;
- case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
- case BOOZ2_AP_MODE_ATTITUDE_CLIMB:
- case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
- break;
- case BOOZ2_AP_MODE_HOVER_DIRECT:
- case BOOZ2_AP_MODE_HOVER_CLIMB:
- case BOOZ2_AP_MODE_HOVER_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER);
- break;
- case BOOZ2_AP_MODE_NAV:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV);
- break;
- default:
- break;
- }
- /* vertical mode */
- switch (new_autopilot_mode) {
- case BOOZ2_AP_MODE_FAILSAFE:
-#ifndef BOOZ_KILL_AS_FAILSAFE
- booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
- break;
-#endif
- case BOOZ2_AP_MODE_KILL:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL);
- break;
- case BOOZ2_AP_MODE_RATE_DIRECT:
- case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
- case BOOZ2_AP_MODE_HOVER_DIRECT:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_DIRECT);
- break;
- case BOOZ2_AP_MODE_RATE_RC_CLIMB:
- case BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_CLIMB);
- break;
- case BOOZ2_AP_MODE_ATTITUDE_CLIMB:
- case BOOZ2_AP_MODE_HOVER_CLIMB:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
- break;
- case BOOZ2_AP_MODE_RATE_Z_HOLD:
- case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
- case BOOZ2_AP_MODE_HOVER_Z_HOLD:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER);
- break;
- case BOOZ2_AP_MODE_NAV:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_NAV);
- break;
- default:
- break;
- }
- booz2_autopilot_mode = new_autopilot_mode;
- }
-
-}
-
-#define THROTTLE_STICK_DOWN() \
- (radio_control.values[RADIO_CONTROL_THROTTLE] <
BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD)
-#define YAW_STICK_PUSHED() \
- (radio_control.values[RADIO_CONTROL_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \
- radio_control.values[RADIO_CONTROL_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)
-
-static inline void booz2_autopilot_check_in_flight( void) {
- if (booz2_autopilot_in_flight) {
- if (booz2_autopilot_in_flight_counter > 0) {
- if (THROTTLE_STICK_DOWN()) {
- booz2_autopilot_in_flight_counter--;
- if (booz2_autopilot_in_flight_counter == 0) {
- booz2_autopilot_in_flight = FALSE;
- }
- }
- else { /* !THROTTLE_STICK_DOWN */
- booz2_autopilot_in_flight_counter = BOOZ2_AUTOPILOT_IN_FLIGHT_TIME;
- }
- }
- }
- else { /* not in flight */
- if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME &&
- booz2_autopilot_motors_on) {
- if (!THROTTLE_STICK_DOWN()) {
- booz2_autopilot_in_flight_counter++;
- if (booz2_autopilot_in_flight_counter ==
BOOZ2_AUTOPILOT_IN_FLIGHT_TIME)
- booz2_autopilot_in_flight = TRUE;
- }
- else { /* THROTTLE_STICK_DOWN */
- booz2_autopilot_in_flight_counter = 0;
- }
- }
- }
-}
-
-static inline void booz2_autopilot_check_motors_on( void ) {
- if (booz2_autopilot_motors_on) {
- if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) {
- if ( booz2_autopilot_motors_on_counter > 0) {
- booz2_autopilot_motors_on_counter--;
- if (booz2_autopilot_motors_on_counter == 0)
- booz2_autopilot_motors_on = FALSE;
- }
- }
- else { /* sticks not in the corner */
- booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME;
- }
- }
- else { /* motors off */
- if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) {
- if ( booz2_autopilot_motors_on_counter < BOOZ2_AUTOPILOT_MOTOR_ON_TIME)
{
- booz2_autopilot_motors_on_counter++;
- if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME)
- booz2_autopilot_motors_on = TRUE;
- }
- }
- else {
- booz2_autopilot_motors_on_counter = 0;
- }
- }
-}
-
-
-
-void booz2_autopilot_on_rc_frame(void) {
-
- uint8_t new_autopilot_mode = 0;
- BOOZ_AP_MODE_OF_PPRZ(radio_control.values[RADIO_CONTROL_MODE],
new_autopilot_mode);
- booz2_autopilot_set_mode(new_autopilot_mode);
-
-#ifdef RADIO_CONTROL_KILL_SWITCH
- if (radio_control.values[RADIO_CONTROL_KILL_SWITCH] < 0)
- booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
-#endif
-
- booz2_autopilot_check_motors_on();
- booz2_autopilot_check_in_flight();
- kill_throttle = !booz2_autopilot_motors_on;
-
- if (booz2_autopilot_mode > BOOZ2_AP_MODE_FAILSAFE) {
- booz2_guidance_v_read_rc();
- booz2_guidance_h_read_rc(booz2_autopilot_in_flight);
- }
-
-}
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -1,117 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef BOOZ2_AUTOPILOT_H
-#define BOOZ2_AUTOPILOT_H
-
-#include "std.h"
-
-#include "led.h"
-
-#include "airframe.h"
-#include "booz2_ins.h"
-
-#define BOOZ2_AP_MODE_FAILSAFE 0
-#define BOOZ2_AP_MODE_KILL 1
-#define BOOZ2_AP_MODE_RATE_DIRECT 2
-#define BOOZ2_AP_MODE_ATTITUDE_DIRECT 3
-#define BOOZ2_AP_MODE_RATE_RC_CLIMB 4
-#define BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB 5
-#define BOOZ2_AP_MODE_ATTITUDE_CLIMB 6
-#define BOOZ2_AP_MODE_RATE_Z_HOLD 7
-#define BOOZ2_AP_MODE_ATTITUDE_Z_HOLD 8
-#define BOOZ2_AP_MODE_HOVER_DIRECT 9
-#define BOOZ2_AP_MODE_HOVER_CLIMB 10
-#define BOOZ2_AP_MODE_HOVER_Z_HOLD 11
-#define BOOZ2_AP_MODE_NAV 12
-
-
-extern uint8_t booz2_autopilot_mode;
-extern uint8_t booz2_autopilot_mode_auto2;
-extern bool_t booz2_autopilot_motors_on;
-extern bool_t booz2_autopilot_in_flight;
-extern bool_t kill_throttle;
-extern bool_t booz2_autopilot_rc;
-
-extern bool_t booz2_autopilot_power_switch;
-
-extern void booz2_autopilot_init(void);
-extern void booz2_autopilot_periodic(void);
-extern void booz2_autopilot_on_rc_frame(void);
-extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
-
-extern bool_t booz2_autopilot_detect_ground;
-extern bool_t booz2_autopilot_detect_ground_once;
-
-extern uint16_t booz2_autopilot_flight_time;
-
-#ifndef BOOZ2_MODE_MANUAL
-#define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
-#endif
-#ifndef BOOZ2_MODE_AUTO1
-#define BOOZ2_MODE_AUTO1 BOOZ2_AP_MODE_ATTITUDE_DIRECT
-#endif
-#ifndef BOOZ2_MODE_AUTO2
-#define BOOZ2_MODE_AUTO2 BOOZ2_AP_MODE_ATTITUDE_Z_HOLD
-#endif
-
-
-#define TRESHOLD_1_PPRZ (MIN_PPRZ / 2)
-#define TRESHOLD_2_PPRZ (MAX_PPRZ / 2)
-
-#define BOOZ_AP_MODE_OF_PPRZ(_rc, _booz_mode) {
\
- if (_rc > TRESHOLD_2_PPRZ) \
- _booz_mode = booz2_autopilot_mode_auto2; \
- else if (_rc > TRESHOLD_1_PPRZ) \
- _booz_mode = BOOZ2_MODE_AUTO1; \
- else \
- _booz_mode = BOOZ2_MODE_MANUAL; \
- }
-
-#define booz2_autopilot_KillThrottle(_v) { \
- kill_throttle = _v;
\
- if (kill_throttle) booz2_autopilot_motors_on = FALSE;
\
- else booz2_autopilot_motors_on = TRUE; \
- }
-
-#define booz2_autopilot_SetPowerSwitch(_v) { \
- booz2_autopilot_power_switch = _v; \
- if (_v) { LED_OFF(POWER_SWITCH_LED); } \
- else { LED_ON(POWER_SWITCH_LED); } \
-}
-
-#ifndef TRESHOLD_GROUND_DETECT
-#define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
-#endif
-static inline void BoozDetectGroundEvent(void) {
- if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE ||
booz2_autopilot_detect_ground_once) {
- if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||
- booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) {
- booz2_autopilot_detect_ground = TRUE;
- booz2_autopilot_detect_ground_once = FALSE;
- }
- }
-}
-
-#endif /* BOOZ2_AUTOPILOT_H */
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-27
21:23:52 UTC (rev 5962)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-27
22:54:54 UTC (rev 5963)
@@ -29,7 +29,7 @@
#include "booz_gps.h"
#include "booz2_ins.h"
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#include "modules.h"
#include "flight_plan.h"
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -34,7 +34,7 @@
#include "booz_radio_control.h"
#endif
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#include "booz_guidance.h"
#include "actuators.h"
Modified: paparazzi3/trunk/sw/airborne/booz/booz_fms.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_fms.h 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/sw/airborne/booz/booz_fms.h 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -26,7 +26,7 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#include "booz_guidance.h"
struct Booz_fms_imu_info {
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_ap.c 2010-09-27 21:23:52 UTC
(rev 5962)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_ap.c 2010-09-27 22:54:54 UTC
(rev 5963)
@@ -26,7 +26,7 @@
#include <inttypes.h>
#include "commands.h"
#include "mercury_xsens.h"
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#include "booz_stabilization.h"
#include "stabilization/booz_stabilization_attitude.h"
#include "led.h"
Copied: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h (from rev
5960, paparazzi3/trunk/sw/airborne/autopilot.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
2010-09-27 22:54:54 UTC (rev 5963)
@@ -0,0 +1,124 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file autopilot.h
+ * \brief Autopilot modes
+ *
+ */
+
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include <inttypes.h>
+#include "std.h"
+#include "sys_time.h"
+
+#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
+
+#define TRESHOLD1 TRESHOLD_MANUAL_PPRZ
+#define TRESHOLD2 (MAX_PPRZ/2)
+
+
+#define PPRZ_MODE_MANUAL 0
+#define PPRZ_MODE_AUTO1 1
+#define PPRZ_MODE_AUTO2 2
+#define PPRZ_MODE_HOME 3
+#define PPRZ_MODE_GPS_OUT_OF_ORDER 4
+#define PPRZ_MODE_NB 5
+
+#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \
+ (pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
+ (pprz > TRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
+
+extern uint8_t pprz_mode;
+extern bool_t kill_throttle;
+
+
+#define LATERAL_MODE_MANUAL 0
+#define LATERAL_MODE_ROLL_RATE 1
+#define LATERAL_MODE_ROLL 2
+#define LATERAL_MODE_COURSE 3
+#define LATERAL_MODE_NB 4
+
+#define STICK_PUSHED(pprz) (pprz < TRESHOLD1 || pprz > TRESHOLD2)
+
+
+#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ *
travel + center)
+
+extern uint8_t fatal_error_nb;
+
+#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
+
+extern uint8_t lateral_mode;
+extern uint8_t vsupply;
+
+extern float slider_1_val, slider_2_val;
+
+extern bool_t launch;
+
+extern uint8_t light_mode;
+extern bool_t gps_lost;
+
+extern bool_t sum_err_reset;
+
+/** Assignment, returning _old_value != _value
+ * Using GCC expression statements */
+#define ModeUpdate(_mode, _value) ({ \
+ uint8_t new_mode = _value; \
+ (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
+})
+
+void periodic_task( void );
+void telecommand_task(void);
+
+#ifdef RADIO_CONTROL
+#include "radio_control.h"
+static inline void autopilot_process_radio_control ( void ) {
+ pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0);
+}
+#endif
+
+extern bool_t power_switch;
+
+#ifdef POWER_SWITCH_LED
+#define autopilot_SetPowerSwitch(_x) { \
+ power_switch = _x; \
+ if (_x) LED_ON(POWER_SWITCH_LED) else LED_OFF(POWER_SWITCH_LED); \
+}
+#else // POWER_SWITCH_LED
+#define autopilot_SetPowerSwitch(_x) { power_switch = _x; }
+#endif // POWER_SWITCH_LED
+
+#define autopilot_ResetFlightTimeAndLaunch(_) { \
+ estimator_flight_time = 0; launch = FALSE; \
+}
+
+
+/* For backward compatibility with old airframe files */
+#include "airframe.h"
+#ifndef CONTROL_RATE
+#define CONTROL_RATE 20
+#endif
+
+#endif /* AUTOPILOT_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c (from rev
5960, paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
2010-09-27 22:54:54 UTC (rev 5963)
@@ -0,0 +1,259 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ *
+ */
+
+#include "autopilot.h"
+
+#include "booz_radio_control.h"
+#include "booz2_commands.h"
+#include "booz2_navigation.h"
+#include "booz_guidance.h"
+#include "booz_stabilization.h"
+#include "led.h"
+
+uint8_t booz2_autopilot_mode;
+uint8_t booz2_autopilot_mode_auto2;
+bool_t booz2_autopilot_motors_on;
+bool_t booz2_autopilot_in_flight;
+uint32_t booz2_autopilot_motors_on_counter;
+uint32_t booz2_autopilot_in_flight_counter;
+bool_t kill_throttle;
+bool_t booz2_autopilot_rc;
+
+bool_t booz2_autopilot_power_switch;
+
+bool_t booz2_autopilot_detect_ground;
+bool_t booz2_autopilot_detect_ground_once;
+
+uint16_t booz2_autopilot_flight_time;
+
+#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40
+#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
+#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
+#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
+
+void booz2_autopilot_init(void) {
+ booz2_autopilot_mode = BOOZ2_AP_MODE_KILL;
+ booz2_autopilot_motors_on = FALSE;
+ booz2_autopilot_in_flight = FALSE;
+ kill_throttle = ! booz2_autopilot_motors_on;
+ booz2_autopilot_motors_on_counter = 0;
+ booz2_autopilot_in_flight_counter = 0;
+ booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
+ booz2_autopilot_detect_ground = FALSE;
+ booz2_autopilot_detect_ground_once = FALSE;
+ booz2_autopilot_flight_time = 0;
+ booz2_autopilot_rc = TRUE;
+ booz2_autopilot_power_switch = FALSE;
+ LED_ON(POWER_SWITCH_LED); // POWER OFF
+}
+
+
+void booz2_autopilot_periodic(void) {
+
+ RunOnceEvery(BOOZ2_NAV_PRESCALER, nav_periodic_task());
+#ifdef BOOZ_FAILSAFE_GROUND_DETECT
+ if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE &&
booz2_autopilot_detect_ground) {
+ booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
+ booz2_autopilot_detect_ground = FALSE;
+ }
+#endif
+ if ( !booz2_autopilot_motors_on ||
+#ifndef BOOZ_FAILSAFE_GROUND_DETECT
+ booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE ||
+#endif
+ booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) {
+ SetCommands(booz2_commands_failsafe,
+ booz2_autopilot_in_flight, booz2_autopilot_motors_on);
+ }
+ else {
+ booz2_guidance_v_run( booz2_autopilot_in_flight );
+ booz2_guidance_h_run( booz2_autopilot_in_flight );
+ SetCommands(booz_stabilization_cmd,
+ booz2_autopilot_in_flight, booz2_autopilot_motors_on);
+ }
+
+}
+
+
+void booz2_autopilot_set_mode(uint8_t new_autopilot_mode) {
+
+ if (new_autopilot_mode != booz2_autopilot_mode) {
+ /* horizontal mode */
+ switch (new_autopilot_mode) {
+ case BOOZ2_AP_MODE_FAILSAFE:
+#ifndef BOOZ_KILL_AS_FAILSAFE
+ booz_stab_att_sp_euler.phi = 0;
+ booz_stab_att_sp_euler.theta = 0;
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
+ break;
+#endif
+ case BOOZ2_AP_MODE_KILL:
+ booz2_autopilot_motors_on = FALSE;
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL);
+ break;
+ case BOOZ2_AP_MODE_RATE_DIRECT:
+ case BOOZ2_AP_MODE_RATE_Z_HOLD:
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE);
+ break;
+ case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
+ case BOOZ2_AP_MODE_ATTITUDE_CLIMB:
+ case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
+ break;
+ case BOOZ2_AP_MODE_HOVER_DIRECT:
+ case BOOZ2_AP_MODE_HOVER_CLIMB:
+ case BOOZ2_AP_MODE_HOVER_Z_HOLD:
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER);
+ break;
+ case BOOZ2_AP_MODE_NAV:
+ booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV);
+ break;
+ default:
+ break;
+ }
+ /* vertical mode */
+ switch (new_autopilot_mode) {
+ case BOOZ2_AP_MODE_FAILSAFE:
+#ifndef BOOZ_KILL_AS_FAILSAFE
+ booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
+ break;
+#endif
+ case BOOZ2_AP_MODE_KILL:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL);
+ break;
+ case BOOZ2_AP_MODE_RATE_DIRECT:
+ case BOOZ2_AP_MODE_ATTITUDE_DIRECT:
+ case BOOZ2_AP_MODE_HOVER_DIRECT:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_DIRECT);
+ break;
+ case BOOZ2_AP_MODE_RATE_RC_CLIMB:
+ case BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_CLIMB);
+ break;
+ case BOOZ2_AP_MODE_ATTITUDE_CLIMB:
+ case BOOZ2_AP_MODE_HOVER_CLIMB:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
+ break;
+ case BOOZ2_AP_MODE_RATE_Z_HOLD:
+ case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD:
+ case BOOZ2_AP_MODE_HOVER_Z_HOLD:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER);
+ break;
+ case BOOZ2_AP_MODE_NAV:
+ booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_NAV);
+ break;
+ default:
+ break;
+ }
+ booz2_autopilot_mode = new_autopilot_mode;
+ }
+
+}
+
+#define THROTTLE_STICK_DOWN() \
+ (radio_control.values[RADIO_CONTROL_THROTTLE] <
BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD)
+#define YAW_STICK_PUSHED() \
+ (radio_control.values[RADIO_CONTROL_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \
+ radio_control.values[RADIO_CONTROL_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)
+
+static inline void booz2_autopilot_check_in_flight( void) {
+ if (booz2_autopilot_in_flight) {
+ if (booz2_autopilot_in_flight_counter > 0) {
+ if (THROTTLE_STICK_DOWN()) {
+ booz2_autopilot_in_flight_counter--;
+ if (booz2_autopilot_in_flight_counter == 0) {
+ booz2_autopilot_in_flight = FALSE;
+ }
+ }
+ else { /* !THROTTLE_STICK_DOWN */
+ booz2_autopilot_in_flight_counter = BOOZ2_AUTOPILOT_IN_FLIGHT_TIME;
+ }
+ }
+ }
+ else { /* not in flight */
+ if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME &&
+ booz2_autopilot_motors_on) {
+ if (!THROTTLE_STICK_DOWN()) {
+ booz2_autopilot_in_flight_counter++;
+ if (booz2_autopilot_in_flight_counter ==
BOOZ2_AUTOPILOT_IN_FLIGHT_TIME)
+ booz2_autopilot_in_flight = TRUE;
+ }
+ else { /* THROTTLE_STICK_DOWN */
+ booz2_autopilot_in_flight_counter = 0;
+ }
+ }
+ }
+}
+
+static inline void booz2_autopilot_check_motors_on( void ) {
+ if (booz2_autopilot_motors_on) {
+ if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) {
+ if ( booz2_autopilot_motors_on_counter > 0) {
+ booz2_autopilot_motors_on_counter--;
+ if (booz2_autopilot_motors_on_counter == 0)
+ booz2_autopilot_motors_on = FALSE;
+ }
+ }
+ else { /* sticks not in the corner */
+ booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME;
+ }
+ }
+ else { /* motors off */
+ if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) {
+ if ( booz2_autopilot_motors_on_counter < BOOZ2_AUTOPILOT_MOTOR_ON_TIME)
{
+ booz2_autopilot_motors_on_counter++;
+ if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME)
+ booz2_autopilot_motors_on = TRUE;
+ }
+ }
+ else {
+ booz2_autopilot_motors_on_counter = 0;
+ }
+ }
+}
+
+
+
+void booz2_autopilot_on_rc_frame(void) {
+
+ uint8_t new_autopilot_mode = 0;
+ BOOZ_AP_MODE_OF_PPRZ(radio_control.values[RADIO_CONTROL_MODE],
new_autopilot_mode);
+ booz2_autopilot_set_mode(new_autopilot_mode);
+
+#ifdef RADIO_CONTROL_KILL_SWITCH
+ if (radio_control.values[RADIO_CONTROL_KILL_SWITCH] < 0)
+ booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);
+#endif
+
+ booz2_autopilot_check_motors_on();
+ booz2_autopilot_check_in_flight();
+ kill_throttle = !booz2_autopilot_motors_on;
+
+ if (booz2_autopilot_mode > BOOZ2_AP_MODE_FAILSAFE) {
+ booz2_guidance_v_read_rc();
+ booz2_guidance_h_read_rc(booz2_autopilot_in_flight);
+ }
+
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h (from rev
5960, paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
2010-09-27 22:54:54 UTC (rev 5963)
@@ -0,0 +1,117 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include "std.h"
+
+#include "led.h"
+
+#include "airframe.h"
+#include "booz2_ins.h"
+
+#define BOOZ2_AP_MODE_FAILSAFE 0
+#define BOOZ2_AP_MODE_KILL 1
+#define BOOZ2_AP_MODE_RATE_DIRECT 2
+#define BOOZ2_AP_MODE_ATTITUDE_DIRECT 3
+#define BOOZ2_AP_MODE_RATE_RC_CLIMB 4
+#define BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB 5
+#define BOOZ2_AP_MODE_ATTITUDE_CLIMB 6
+#define BOOZ2_AP_MODE_RATE_Z_HOLD 7
+#define BOOZ2_AP_MODE_ATTITUDE_Z_HOLD 8
+#define BOOZ2_AP_MODE_HOVER_DIRECT 9
+#define BOOZ2_AP_MODE_HOVER_CLIMB 10
+#define BOOZ2_AP_MODE_HOVER_Z_HOLD 11
+#define BOOZ2_AP_MODE_NAV 12
+
+
+extern uint8_t booz2_autopilot_mode;
+extern uint8_t booz2_autopilot_mode_auto2;
+extern bool_t booz2_autopilot_motors_on;
+extern bool_t booz2_autopilot_in_flight;
+extern bool_t kill_throttle;
+extern bool_t booz2_autopilot_rc;
+
+extern bool_t booz2_autopilot_power_switch;
+
+extern void booz2_autopilot_init(void);
+extern void booz2_autopilot_periodic(void);
+extern void booz2_autopilot_on_rc_frame(void);
+extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
+
+extern bool_t booz2_autopilot_detect_ground;
+extern bool_t booz2_autopilot_detect_ground_once;
+
+extern uint16_t booz2_autopilot_flight_time;
+
+#ifndef BOOZ2_MODE_MANUAL
+#define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
+#endif
+#ifndef BOOZ2_MODE_AUTO1
+#define BOOZ2_MODE_AUTO1 BOOZ2_AP_MODE_ATTITUDE_DIRECT
+#endif
+#ifndef BOOZ2_MODE_AUTO2
+#define BOOZ2_MODE_AUTO2 BOOZ2_AP_MODE_ATTITUDE_Z_HOLD
+#endif
+
+
+#define TRESHOLD_1_PPRZ (MIN_PPRZ / 2)
+#define TRESHOLD_2_PPRZ (MAX_PPRZ / 2)
+
+#define BOOZ_AP_MODE_OF_PPRZ(_rc, _booz_mode) {
\
+ if (_rc > TRESHOLD_2_PPRZ) \
+ _booz_mode = booz2_autopilot_mode_auto2; \
+ else if (_rc > TRESHOLD_1_PPRZ) \
+ _booz_mode = BOOZ2_MODE_AUTO1; \
+ else \
+ _booz_mode = BOOZ2_MODE_MANUAL; \
+ }
+
+#define autopilot_KillThrottle(_v) { \
+ kill_throttle = _v;
\
+ if (kill_throttle) booz2_autopilot_motors_on = FALSE;
\
+ else booz2_autopilot_motors_on = TRUE; \
+ }
+
+#define autopilot_SetPowerSwitch(_v) { \
+ booz2_autopilot_power_switch = _v; \
+ if (_v) { LED_OFF(POWER_SWITCH_LED); } \
+ else { LED_ON(POWER_SWITCH_LED); } \
+}
+
+#ifndef TRESHOLD_GROUND_DETECT
+#define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
+#endif
+static inline void BoozDetectGroundEvent(void) {
+ if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE ||
booz2_autopilot_detect_ground_once) {
+ if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||
+ booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) {
+ booz2_autopilot_detect_ground = TRUE;
+ booz2_autopilot_detect_ground_once = FALSE;
+ }
+ }
+}
+
+#endif /* AUTOPILOT_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-27
21:23:52 UTC (rev 5962)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-27
22:54:54 UTC (rev 5963)
@@ -47,7 +47,7 @@
#include "booz2_battery.h"
#include "booz_fms.h"
-#include "booz2_autopilot.h"
+#include "autopilot.h"
#include "booz_stabilization.h"
#include "booz_guidance.h"
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-27
21:23:52 UTC (rev 5962)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-27
22:54:54 UTC (rev 5963)
@@ -30,7 +30,7 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
-#include "booz/booz2_autopilot.h"
+#include "autopilot.h"
#include "booz/booz_stabilization.h"
#include "booz/booz_guidance.h"
#include "booz/booz2_navigation.h"
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5963] starting to merge booz, moving booz2_autopilot to firmwares/ rotorcraft/autopilot,
Felix Ruess <=