diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml index 8405c3add6..cbc48adfdb 100644 --- a/conf/airframes/CDW/debug_i2c.xml +++ b/conf/airframes/CDW/debug_i2c.xml @@ -213,7 +213,6 @@ - diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index e8401c7ef6..5b0d0b2ed0 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -11,7 +11,6 @@ - diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 2c55323d36..ed983ce12e 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -15,7 +15,6 @@ - diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml index 1a4ac65bc3..e3774fe6d2 100644 --- a/conf/airframes/ENAC/quadrotor/navgo.xml +++ b/conf/airframes/ENAC/quadrotor/navgo.xml @@ -10,7 +10,6 @@ - @@ -105,7 +104,6 @@
- diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index 09e9ba1a0e..33cbbb872a 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -242,7 +242,6 @@ - diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index aee822cb0a..5be5972e59 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -214,7 +214,6 @@ - diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 84d3d6e6a7..cbde578834 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -16,6 +16,7 @@ + @@ -108,39 +109,38 @@
-
- +
- - - - - - - + + + + + + + - - - + + + - - - + + + - - - + + +
@@ -204,7 +204,7 @@ - +
diff --git a/conf/airframes/ENAC/quadrotor/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml similarity index 100% rename from conf/airframes/ENAC/quadrotor/g1_vision.xml rename to conf/airframes/obsolete/ENAC/g1_vision.xml diff --git a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml similarity index 100% rename from conf/airframes/ENAC/quadrotor/mkk1-vision.xml rename to conf/airframes/obsolete/ENAC/mkk1-vision.xml diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml similarity index 100% rename from conf/airframes/ENAC/quadrotor/mkk1.xml rename to conf/airframes/obsolete/ENAC/mkk1.xml diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml similarity index 100% rename from conf/airframes/ENAC/quadrotor/nova1.xml rename to conf/airframes/obsolete/ENAC/nova1.xml diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml similarity index 100% rename from conf/airframes/UofAdelaide/A1000_BOOZ.xml rename to conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml similarity index 100% rename from conf/airframes/UofAdelaide/A1000_LISA.xml rename to conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml similarity index 100% rename from conf/airframes/UofAdelaide/A1000_NOVA.xml rename to conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml similarity index 100% rename from conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml rename to conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml similarity index 100% rename from conf/airframes/UofAdelaide/booz2_a1000.xml rename to conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml similarity index 100% rename from conf/airframes/UofAdelaide/lisa_a1000.xml rename to conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml diff --git a/conf/boards/lisa_m_2.0.makefile b/conf/boards/lisa_m_2.0.makefile index 8040376e97..dceee9636e 100644 --- a/conf/boards/lisa_m_2.0.makefile +++ b/conf/boards/lisa_m_2.0.makefile @@ -2,11 +2,11 @@ # # lisa_m_2.0.makefile # -# http://paparazzi.enac.fr/wiki/Lisa/M +# http://paparazzi.enac.fr/wiki/Lisa/M_v20 # BOARD=lisa_m -BOARD_VERSION=1.0 +BOARD_VERSION=2.0 BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" ARCH=stm32 diff --git a/conf/modules/bat_checker.xml b/conf/modules/bat_checker.xml new file mode 100644 index 0000000000..74160f19e9 --- /dev/null +++ b/conf/modules/bat_checker.xml @@ -0,0 +1,21 @@ + + + + +
+ +
+ + + + + + + + diff --git a/conf/modules/battery_buzzer.xml b/conf/modules/battery_buzzer.xml deleted file mode 100644 index 478b760978..0000000000 --- a/conf/modules/battery_buzzer.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - -
- -
- - - - - -ifneq ($(ARCH), stm32) -$(error The battery_buzzer module is currently only implemented for STM32 based boards.) -endif - - - - - diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index c745b63d27..321a47247f 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -43,6 +43,7 @@ + diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index 231412d5de..dc3b22f357 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -24,6 +24,7 @@ #define LED_3_GPIO_PIN GPIO_Pin_2 #define LED_3_AFIO_REMAP ((void)0) +// GPIO pins #define LED_4_BANK #define LED_4_GPIO GPIOC #define LED_4_GPIO_CLK RCC_APB2Periph_GPIOC @@ -42,29 +43,49 @@ #define IMU_ACC_DRDY_GPIO GPIOB #define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB -/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ -#ifndef ADC_CHANNEL_VSUPPLY -#define ADC_CHANNEL_VSUPPLY 2 -#endif + + #define DefaultVoltageOfAdc(adc) (0.00485*adc) /* Onboard ADCs */ /* - ADC1 PC3/ADC13 - ADC2 PA0/ADC0 - ADC3 PC0/ADC10 - ADC4 PC1/ADC11 - ADC5 PC5/ADC15 - ADC6 PA1/ADC1 - ADC7 PC2/ADC12 - BATT PC4/ADC14 + ADC_1 PC3/ADC13 + ADC_2 PC0/ADC10 + ADC_3 PC1/ADC11 + ADC_4 PC5/ADC15 + ADC_6 PC2/ADC12 + BATT PC4/ADC14 */ #define BOARD_ADC_CHANNEL_1 ADC_Channel_13 -#define BOARD_ADC_CHANNEL_2 ADC_Channel_0 -// FIXME - removed for now and used for battery monitoring -//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10 -#define BOARD_ADC_CHANNEL_3 ADC_Channel_14 -#define BOARD_ADC_CHANNEL_4 ADC_Channel_11 +#define BOARD_ADC_CHANNEL_2 ADC_Channel_10 +#define BOARD_ADC_CHANNEL_3 ADC_Channel_11 +// we can only use ADC1,2,3; the last channel is for bat monitoring +#define BOARD_ADC_CHANNEL_4 ADC_Channel_14 + +/* provide defines that can be used to access the ADC_x in the code or airframe file + * these directly map to the index number of the 4 adc channels defined above + * 4th (index 3) is used for bat monitoring by default + */ +#define ADC_1 0 +#define ADC_2 1 +#define ADC_3 2 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY 3 +#endif + +/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ +// FIXME, this is not very nice, is also stm lib specific +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + (gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4; \ + (gpio).GPIO_Mode = GPIO_Mode_AIN; \ + GPIO_Init(GPIOC, (&gpio)); \ + } +#endif // USE_AD1 + + #define BOARD_HAS_BARO 1 diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h new file mode 100644 index 0000000000..5467a986cf --- /dev/null +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -0,0 +1,107 @@ +#ifndef CONFIG_LISA_M_2_0_H +#define CONFIG_LISA_M_2_0_H + +#define BOARD_LISA_M + +#define AHB_CLK 72000000 + +/* Onboard LEDs */ +#define LED_1_BANK +#define LED_1_GPIO GPIOB +#define LED_1_GPIO_CLK RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO +#define LED_1_GPIO_PIN GPIO_Pin_4 +#define LED_1_AFIO_REMAP GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE) + +/* shared with ADC15 */ +#define LED_2_BANK +#define LED_2_GPIO GPIOC +#define LED_2_GPIO_CLK RCC_APB2Periph_GPIOC +#define LED_2_GPIO_PIN GPIO_Pin_5 +#define LED_2_AFIO_REMAP ((void)0) + +#define LED_3_BANK +#define LED_3_GPIO GPIOC +#define LED_3_GPIO_CLK RCC_APB2Periph_GPIOC +#define LED_3_GPIO_PIN GPIO_Pin_2 +#define LED_3_AFIO_REMAP ((void)0) + +#define LED_4_BANK +#define LED_4_GPIO GPIOC +#define LED_4_GPIO_CLK RCC_APB2Periph_GPIOC +#define LED_4_GPIO_PIN GPIO_Pin_12 +#define LED_4_AFIO_REMAP ((void)0) + +#define LED_5_BANK +#define LED_5_GPIO GPIOC +#define LED_5_GPIO_CLK RCC_APB2Periph_GPIOC +#define LED_5_GPIO_PIN GPIO_Pin_10 +#define LED_5_AFIO_REMAP ((void)0) + + +/* configuration for aspirin - and more generaly IMUs */ +#define IMU_ACC_DRDY_RCC_GPIO RCC_APB2Periph_GPIOB +#define IMU_ACC_DRDY_GPIO GPIOB +#define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB + + + +#define DefaultVoltageOfAdc(adc) (0.00485*adc) + +/* Onboard ADCs */ +/* + ADC1 PC3/ADC13 + ADC2 PC0/ADC10 + ADC3 PC1/ADC11 + ADC4 PC5/ADC15 + ADC6 PC2/ADC12 + BATT PC4/ADC14 +*/ +#define BOARD_ADC_CHANNEL_1 ADC_Channel_13 +#define BOARD_ADC_CHANNEL_2 ADC_Channel_10 +#define BOARD_ADC_CHANNEL_3 ADC_Channel_11 +// we can only use ADC1,2,3; the last channel is for bat monitoring +#define BOARD_ADC_CHANNEL_4 ADC_Channel_14 + +/* provide defines that can be used to access the ADC_x in the code or airframe file + * these directly map to the index number of the 4 adc channels defined above + * 4th (index 3) is used for bat monitoring by default + */ +#define ADC_1 0 +#define ADC_2 1 +#define ADC_3 2 + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY 3 +#endif + +/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ +// FIXME, this is not very nice, is also stm lib specific +#ifdef USE_AD1 +#define ADC1_GPIO_INIT(gpio) { \ + (gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4; \ + (gpio).GPIO_Mode = GPIO_Mode_AIN; \ + GPIO_Init(GPIOC, (&gpio)); \ + } +#endif // USE_AD1 + + + +#define BOARD_HAS_BARO 1 + +#define USE_OPENCM3 1 + +// not needed with USE_OPENCM3: +//#define HSE_TYPE_EXT_CLK +//#define STM32_RCC_MODE RCC_HSE_ON +//#define STM32_PLL_MULT RCC_PLLMul_6 + +#define PWM_5AND6_TIMER TIM5 +#define PWM_5AND6_RCC RCC_APB1Periph_TIM5 +#define PWM5_OC 1 +#define PWM6_OC 2 +#define PWM_5AND6_GPIO GPIOA +#define PWM5_Pin GPIO_Pin_0 +#define PWM6_Pin GPIO_Pin_1 + +#endif /* CONFIG_LISA_M_2_0_H */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 92271f6425..7742cb9b9f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2008-2012 The Paparazzi Team * * This file is part of paparazzi. * @@ -19,7 +17,6 @@ * 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 "firmwares/rotorcraft/autopilot.h" @@ -33,41 +30,47 @@ uint8_t autopilot_mode; uint8_t autopilot_mode_auto2; -bool_t autopilot_motors_on; -bool_t autopilot_in_flight; -uint32_t autopilot_motors_on_counter; -uint32_t autopilot_in_flight_counter; -uint8_t autopilot_check_motor_status; -bool_t kill_throttle; -bool_t autopilot_rc; +bool_t autopilot_in_flight; +uint32_t autopilot_in_flight_counter; +uint16_t autopilot_flight_time; + +bool_t autopilot_motors_on; +bool_t kill_throttle; + +bool_t autopilot_rc; bool_t autopilot_power_switch; bool_t autopilot_detect_ground; bool_t autopilot_detect_ground_once; -uint16_t autopilot_flight_time; - -#define AUTOPILOT_MOTOR_ON_TIME 40 #define AUTOPILOT_IN_FLIGHT_TIME 40 -#define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20) -#define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20) -// Motors ON check state machine -#define STATUS_MOTORS_OFF 0 -#define STATUS_M_OFF_STICK_PUSHED 1 -#define STATUS_START_MOTORS 2 -#define STATUS_MOTORS_ON 3 -#define STATUS_M_ON_STICK_PUSHED 4 -#define STATUS_STOP_MOTORS 5 + +#ifndef AUTOPILOT_DISABLE_AHRS_KILL +#include "subsystems/ahrs.h" +static inline int ahrs_is_aligned(void) { + return (ahrs.status == AHRS_RUNNING); +} +#else +static inline int ahrs_is_aligned(void) { + return TRUE; +} +#endif + +#if USE_KILL_SWITCH_FOR_MOTOR_ARMING +#include "autopilot_arming_switch.h" +#elif USE_THROTTLE_FOR_MOTOR_ARMING +#include "autopilot_arming_throttle.h" +#else +#include "autopilot_arming_yaw.h" +#endif void autopilot_init(void) { autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; - autopilot_in_flight = FALSE; kill_throttle = ! autopilot_motors_on; - autopilot_motors_on_counter = 0; + autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; - autopilot_check_motor_status = STATUS_MOTORS_OFF; autopilot_mode_auto2 = MODE_AUTO2; autopilot_detect_ground = FALSE; autopilot_detect_ground_once = FALSE; @@ -77,6 +80,7 @@ void autopilot_init(void) { #ifdef POWER_SWITCH_LED LED_ON(POWER_SWITCH_LED); // POWER OFF #endif + autopilot_arming_init(); } @@ -187,12 +191,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { } -#define THROTTLE_STICK_DOWN() \ - (radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_TRESHOLD) -#define YAW_STICK_PUSHED() \ - (radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \ - radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD) - static inline void autopilot_check_in_flight( bool_t motors_on ) { if (autopilot_in_flight) { @@ -223,77 +221,14 @@ static inline void autopilot_check_in_flight( bool_t motors_on ) { } } -#ifdef AUTOPILOT_KILL_WITHOUT_AHRS -#include "subsystems/ahrs.h" -static inline int ahrs_is_aligned(void) { - return (ahrs.status == AHRS_RUNNING); -} -#else -static inline int ahrs_is_aligned(void) { - return TRUE; -} -#endif -/** Set motors ON or OFF and change the status of the check_motors state machine - */ void autopilot_set_motors_on(bool_t motors_on) { - autopilot_motors_on = motors_on; + if (ahrs_is_aligned() && motors_on) + autopilot_motors_on = TRUE; + else + autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; - if (autopilot_motors_on) autopilot_check_motor_status = STATUS_MOTORS_ON; - else autopilot_check_motor_status = STATUS_MOTORS_OFF; -} - -/** - * State machine to check if motors should be turned ON or OFF - * The motors start/stop when pushing the yaw stick without throttle during a given time - * An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed - * The stick must return to a neutral position before starting/stoping again - */ -static inline void autopilot_check_motors_on( void ) { - switch(autopilot_check_motor_status) { - case STATUS_MOTORS_OFF: - autopilot_motors_on = FALSE; - autopilot_motors_on_counter = 0; - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed - autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; - break; - case STATUS_M_OFF_STICK_PUSHED: - autopilot_motors_on = FALSE; - autopilot_motors_on_counter++; - if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME) - autopilot_check_motor_status = STATUS_START_MOTORS; - else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon - autopilot_check_motor_status = STATUS_MOTORS_OFF; - break; - case STATUS_START_MOTORS: - autopilot_motors_on = TRUE; - autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released - autopilot_check_motor_status = STATUS_MOTORS_ON; - break; - case STATUS_MOTORS_ON: - autopilot_motors_on = TRUE; - autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed - autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED; - break; - case STATUS_M_ON_STICK_PUSHED: - autopilot_motors_on = TRUE; - autopilot_motors_on_counter--; - if (autopilot_motors_on_counter == 0) - autopilot_check_motor_status = STATUS_STOP_MOTORS; - else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon - autopilot_check_motor_status = STATUS_MOTORS_ON; - break; - case STATUS_STOP_MOTORS: - autopilot_motors_on = FALSE; - autopilot_motors_on_counter = 0; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released - autopilot_check_motor_status = STATUS_MOTORS_OFF; - break; - default: - break; - }; + autopilot_arming_set(autopilot_motors_on); } @@ -303,30 +238,18 @@ void autopilot_on_rc_frame(void) { AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode); autopilot_set_mode(new_autopilot_mode); -#ifdef RADIO_KILL_SWITCH - if (radio_control.values[RADIO_KILL_SWITCH] < 0) + if (kill_switch_is_on()) autopilot_set_mode(AP_MODE_KILL); -#endif -#ifdef AUTOPILOT_KILL_WITHOUT_AHRS if (!ahrs_is_aligned()) autopilot_set_mode(AP_MODE_KILL); -#endif -#ifdef NO_ARMING_SEQUENCE -#ifndef RADIO_KILL_SWITCH - // no arming sequence and no kill switch - // motors are turned on when "in_flight" is detected - // it can also be set by the flight plan - autopilot_check_in_flight(TRUE); - autopilot_motors_on = autopilot_in_flight; -#endif -#else - // an arming sequence is used to start motors - autopilot_check_motors_on(); + // an arming sequence is used to start/stop motors + autopilot_arming_check_motors_on(); + + kill_throttle = ! autopilot_motors_on; + autopilot_check_in_flight(autopilot_motors_on); -#endif - kill_throttle = !autopilot_motors_on; if (autopilot_mode > AP_MODE_FAILSAFE) { guidance_v_read_rc(); diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 82cf02b576..2e1cdcde22 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -19,7 +17,6 @@ * 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 @@ -50,8 +47,8 @@ extern uint8_t autopilot_mode; extern uint8_t autopilot_mode_auto2; -extern bool_t autopilot_motors_on; -extern bool_t autopilot_in_flight; +extern bool_t autopilot_motors_on; +extern bool_t autopilot_in_flight; extern bool_t kill_throttle; extern bool_t autopilot_rc; @@ -68,6 +65,8 @@ extern bool_t autopilot_detect_ground_once; extern uint16_t autopilot_flight_time; + + #ifndef MODE_MANUAL #define MODE_MANUAL AP_MODE_RATE_DIRECT #endif @@ -91,10 +90,8 @@ extern uint16_t autopilot_flight_time; _mode = MODE_MANUAL; \ } -#define autopilot_KillThrottle(_v) { \ - kill_throttle = _v; \ - if (kill_throttle) autopilot_motors_on = FALSE; \ - else autopilot_motors_on = TRUE; \ +#define autopilot_KillThrottle(_v) { \ + autopilot_set_motors_on(_v); \ } #ifdef POWER_SWITCH_LED diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h new file mode 100644 index 0000000000..e43110e03f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef AUTOPILOT_ARMING_SWITCH_H +#define AUTOPILOT_ARMING_SWITCH_H + +#include "autopilot_rc_helpers.h" + +#ifndef RADIO_KILL_SWITCH +#error "You need to have a RADIO_KILL_SWITCH configured to arm the motors with the switch!" +#endif + +enum arming_state { + STATE_UNINIT, + STATE_WAITING, + STATE_STARTABLE, + STATE_MOTORS_ON +}; + +enum arming_state autopilot_arming_state; + +static inline void autopilot_arming_init(void) { + autopilot_arming_state = STATE_UNINIT; +} + +static inline void autopilot_arming_set(bool_t motors_on) { + if (motors_on) { + autopilot_arming_state = STATE_MOTORS_ON; + } + else { + if (autopilot_arming_state == STATE_MOTORS_ON) + autopilot_arming_state = STATE_STARTABLE; + } +} + +/** + * State machine to check if motors should be turned ON or OFF using the kill switch. + * If kill switch is off during startup (unkilled), you need to kill again first, + * then unkill to start. + * Also to start the motors, throttle needs to be down, other sticks centered, + * AHRS aligned and you need be in manual mode. + */ +static inline void autopilot_arming_check_motors_on( void ) { + switch(autopilot_arming_state) { + case STATE_UNINIT: + autopilot_motors_on = FALSE; + if (kill_switch_is_on()) + autopilot_arming_state = STATE_STARTABLE; + else + autopilot_arming_state = STATE_WAITING; + break; + case STATE_WAITING: + autopilot_motors_on = FALSE; + if (kill_switch_is_on()) + autopilot_arming_state = STATE_STARTABLE; + break; + case STATE_STARTABLE: + autopilot_motors_on = FALSE; + if (!kill_switch_is_on() && + THROTTLE_STICK_DOWN() && + rc_attitude_sticks_centered() && + autopilot_mode == MODE_MANUAL && + ahrs_is_aligned()) { + autopilot_arming_state = STATE_MOTORS_ON; + } + break; + case STATE_MOTORS_ON: + autopilot_motors_on = TRUE; + if (kill_switch_is_on()) + autopilot_arming_state = STATE_STARTABLE; + break; + default: + break; + } + kill_throttle = ! autopilot_motors_on; +} + +#endif /* AUTOPILOT_ARMING_SWITCH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h new file mode 100644 index 0000000000..679055860b --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef AUTOPILOT_ARMING_THROTTLE_H +#define AUTOPILOT_ARMING_THROTTLE_H + +#include "autopilot_rc_helpers.h" + +#define AUTOPILOT_ARMING_DELAY 10 + +enum arming_throttle_state { + STATE_UNINIT, + STATE_WAITING, + STATE_MOTORS_OFF_READY, + STATE_ARMING, + STATE_MOTORS_ON, + STATE_UNARMING +}; + +enum arming_throttle_state autopilot_arming_state; +uint8_t autopilot_arming_delay_counter; + +static inline void autopilot_arming_init(void) { + autopilot_arming_state = STATE_UNINIT; + autopilot_arming_delay_counter = 0; +} + +static inline void autopilot_arming_set(bool_t motors_on) { + if (motors_on) { + autopilot_arming_state = STATE_MOTORS_ON; + } + else { + if (autopilot_arming_state == STATE_MOTORS_ON) + autopilot_arming_state = STATE_MOTORS_OFF_READY; + } +} + +/** + * State machine to check if motors should be turned ON or OFF. + * - automatically unkill when applying throttle + * - if throttle was not down at startup, you need to put throttle down again first + * - other sticks need to be centered to start motors + * - need to be in manual mode to start the motors + * - AHRS needs to be aligned + */ +static inline void autopilot_arming_check_motors_on( void ) { + switch(autopilot_arming_state) { + case STATE_UNINIT: + autopilot_motors_on = FALSE; + autopilot_arming_delay_counter = 0; + if (THROTTLE_STICK_DOWN()) + autopilot_arming_state = STATE_MOTORS_OFF_READY; + else + autopilot_arming_state = STATE_WAITING; + break; + case STATE_WAITING: + autopilot_motors_on = FALSE; + autopilot_arming_delay_counter = 0; + if (THROTTLE_STICK_DOWN()) + autopilot_arming_state = STATE_MOTORS_OFF_READY; + break; + case STATE_MOTORS_OFF_READY: + autopilot_motors_on = FALSE; + autopilot_arming_delay_counter = 0; + if (!THROTTLE_STICK_DOWN() && + rc_attitude_sticks_centered() && + autopilot_mode == MODE_MANUAL && + ahrs_is_aligned()) { + autopilot_arming_state = STATE_ARMING; + } + break; + case STATE_ARMING: + autopilot_motors_on = FALSE; + autopilot_arming_delay_counter++; + if (THROTTLE_STICK_DOWN() || + !rc_attitude_sticks_centered() || + autopilot_mode != MODE_MANUAL || + !ahrs_is_aligned()) { + autopilot_arming_state = STATE_MOTORS_OFF_READY; + } + else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) + autopilot_arming_state = STATE_MOTORS_ON; + break; + case STATE_MOTORS_ON: + autopilot_motors_on = TRUE; + autopilot_arming_delay_counter = AUTOPILOT_ARMING_DELAY; + if (THROTTLE_STICK_DOWN()) + autopilot_arming_state = STATE_UNARMING; + break; + case STATE_UNARMING: + autopilot_motors_on = TRUE; + autopilot_arming_delay_counter--; + if (!THROTTLE_STICK_DOWN()) + autopilot_arming_state = STATE_MOTORS_ON; + else if (autopilot_arming_delay_counter == 0) + autopilot_arming_state = STATE_MOTORS_OFF_READY; + break; + default: + break; + } + +} + +#endif /* AUTOPILOT_ARMING_THROTTLE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h new file mode 100644 index 0000000000..daa9b741dd --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef AUTOPILOT_ARMING_YAW_H +#define AUTOPILOT_ARMING_YAW_H + +#include "autopilot_rc_helpers.h" + + +#define AUTOPILOT_MOTOR_ON_TIME 40 + +// Motors ON check state machine +enum arming_state { + STATUS_MOTORS_OFF, + STATUS_M_OFF_STICK_PUSHED, + STATUS_START_MOTORS, + STATUS_MOTORS_ON, + STATUS_M_ON_STICK_PUSHED, + STATUS_STOP_MOTORS +}; + +uint32_t autopilot_motors_on_counter; +enum arming_state autopilot_check_motor_status; + + +static inline void autopilot_arming_init(void) { + autopilot_motors_on_counter = 0; + autopilot_check_motor_status = STATUS_MOTORS_OFF; +} + + +/** Update the status of the check_motors state machine. + */ +static inline void autopilot_arming_set(bool_t motors_on) { + if (motors_on) + autopilot_check_motor_status = STATUS_MOTORS_ON; + else + autopilot_check_motor_status = STATUS_MOTORS_OFF; +} + +/** + * State machine to check if motors should be turned ON or OFF. + * The motors start/stop when pushing the yaw stick without throttle during a given time + * An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed + * The stick must return to a neutral position before starting/stoping again + */ +static inline void autopilot_arming_check_motors_on( void ) { + switch(autopilot_check_motor_status) { + case STATUS_MOTORS_OFF: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter = 0; + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; + break; + case STATUS_M_OFF_STICK_PUSHED: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter++; + if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME) + autopilot_check_motor_status = STATUS_START_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + case STATUS_START_MOTORS: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_MOTORS_ON: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED; + break; + case STATUS_M_ON_STICK_PUSHED: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter--; + if (autopilot_motors_on_counter == 0) + autopilot_check_motor_status = STATUS_STOP_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_STOP_MOTORS: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter = 0; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + default: + break; + }; +} + +#endif /* AUTOPILOT_ARMING_YAW_H */ diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h new file mode 100644 index 0000000000..a9ec71c3de --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef AUTOPILOT_RC_HELPERS_H +#define AUTOPILOT_RC_HELPERS_H + +#include "subsystems/radio_control.h" + +#define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20) +#define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20) +#ifndef AUTOPILOT_STICK_CENTER_TRESHOLD +#define AUTOPILOT_STICK_CENTER_TRESHOLD (MAX_PPRZ * 1 / 20) +#endif + +#define THROTTLE_STICK_DOWN() \ + (radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_TRESHOLD) +#define YAW_STICK_PUSHED() \ + (radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \ + radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD) +#define YAW_STICK_CENTERED() \ + (radio_control.values[RADIO_YAW] < AUTOPILOT_STICK_CENTER_TRESHOLD && \ + radio_control.values[RADIO_YAW] > -AUTOPILOT_STICK_CENTER_TRESHOLD) +#define PITCH_STICK_CENTERED() \ + (radio_control.values[RADIO_PITCH] < AUTOPILOT_STICK_CENTER_TRESHOLD && \ + radio_control.values[RADIO_PITCH] > -AUTOPILOT_STICK_CENTER_TRESHOLD) +#define ROLL_STICK_CENTERED() \ + (radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_TRESHOLD && \ + radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_TRESHOLD) + +static inline bool_t rc_attitude_sticks_centered(void) { + return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED(); +} + +#ifdef RADIO_KILL_SWITCH +static inline bool_t kill_switch_is_on(void) { + if (radio_control.values[RADIO_KILL_SWITCH] < 0) + return TRUE; + else + return FALSE; +} +#else +static inline bool_t kill_switch_is_on(void) { + return FALSE; +} +#endif + + +#endif /* AUTOPILOT_RC_HELPERS_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 904891a145..ab0a4c90f0 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -48,6 +48,10 @@ #warning "ALL control gains are now positive!!!" #endif +#if defined GUIDANCE_V_INV_M +#warning "GUIDANCE_V_INV_M has been removed. If you don't want to use adaptive hover, please define GUIDANCE_V_NOMINAL_HOVER_THROTTLE" +#endif + #define GUIDANCE_V_GAIN_SCALER 48 uint8_t guidance_v_mode; @@ -55,6 +59,8 @@ int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; int32_t guidance_v_delta_t; +int16_t guidance_v_nominal_throttle; + /** Direct throttle from radio control. * range 0:#MAX_PPRZ @@ -101,6 +107,10 @@ void guidance_v_init(void) { guidance_v_z_sum_err = 0; +#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE + guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE * MAX_PPRZ; +#endif + gv_adapt_init(); } @@ -122,10 +132,15 @@ void guidance_v_mode_changed(uint8_t new_mode) { return; switch (new_mode) { + case GUIDANCE_V_MODE_HOVER: + guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint + guidance_v_z_sum_err = 0; + GuidanceVSetRef(ins_ltp_pos.z, 0, 0); + break; case GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_CLIMB: - case GUIDANCE_V_MODE_HOVER: + guidance_v_zd_sp = 0; case GUIDANCE_V_MODE_NAV: guidance_v_z_sum_err = 0; GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0); @@ -158,8 +173,7 @@ void guidance_v_run(bool_t in_flight) { switch (guidance_v_mode) { case GUIDANCE_V_MODE_RC_DIRECT: - guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that - GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ? + guidance_v_z_sp = ins_ltp_pos.z; // for display only stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; @@ -260,8 +274,8 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig guidance_v_z_sum_err = 0; /* our nominal command : (g + zdd)*m */ -#ifdef GUIDANCE_V_INV_M - const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, GV_ADAPT_X_FRAC); +#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE + const int32_t inv_m = BFP_OF_REAL(9.81/guidance_v_nominal_throttle, FF_CMD_FRAC); #else const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC); #endif @@ -280,11 +294,11 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig /* our error feed back command */ /* z-axis pointing down -> positive error means we need less thrust */ - guidance_v_fb_cmd = ((-guidance_v_kp * GUIDANCE_V_GAIN_SCALER * err_z) >> 12) + - ((-guidance_v_kd * GUIDANCE_V_GAIN_SCALER * err_zd) >> 21) + - ((-guidance_v_ki * GUIDANCE_V_GAIN_SCALER * guidance_v_z_sum_err) >> 21); + guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) + + ((-guidance_v_kd * err_zd) >> 21) + + ((-guidance_v_ki * guidance_v_z_sum_err) >> 21); - guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd; + guidance_v_delta_t = guidance_v_ff_cmd + (guidance_v_fb_cmd * GUIDANCE_V_GAIN_SCALER); /* bound the result */ Bound(guidance_v_delta_t, 0, MAX_PPRZ); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index ded9bd3500..7a2c7e5872 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -94,6 +94,11 @@ extern int32_t guidance_v_fb_cmd; ///< feed-back command */ extern int32_t guidance_v_delta_t; +/** nominal throttle for hover. + * range: 0 : #MAX_PPRZ + */ +extern int16_t guidance_v_nominal_throttle; + extern int32_t guidance_v_kp; ///< vertical control P-gain extern int32_t guidance_v_kd; ///< vertical control D-gain extern int32_t guidance_v_ki; ///< vertical control I-gain @@ -109,5 +114,10 @@ extern void guidance_v_run(bool_t in_flight); guidance_v_z_sum_err = 0; \ } +#define guidance_v_SetNominalHoverThrottle(_throttle) { \ + guidance_v_nominal_throttle = _throttle; \ + Bound(guidance_v_nominal_throttle, 0.1*MAX_PPRZ, 0.9*MAX_PPRZ); \ + } + #endif /* GUIDANCE_V */ diff --git a/sw/airborne/modules/bat_checker/bat_checker.c b/sw/airborne/modules/bat_checker/bat_checker.c new file mode 100644 index 0000000000..3b9dd7e52e --- /dev/null +++ b/sw/airborne/modules/bat_checker/bat_checker.c @@ -0,0 +1,79 @@ +/* + * $Id$ + * + * Copyright (C) 2012 Thomas Kolb + * + * 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 "bat_checker.h" +#include "generated/airframe.h" +#include "subsystems/electrical.h" +#include "led.h" + +#ifndef CRITIC_BAT_LEVEL +#error You must define CRITIC_BAT_LEVEL to use this module! +#endif + +#ifndef LOW_BAT_LEVEL +#error You must define LOW_BAT_LEVEL to use this module! +#endif + +#ifndef BAT_CHECKER_LED +#error You must define BAT_CHECKER_LED in your airframe file. +#endif + +#ifndef BAT_CHECKER_DELAY +#warning BAT_CHECKER_DELAY is undefined. Falling back to 5 seconds. +#define BAT_CHECKER_DELAY 5 +#endif + +// at this level, the buzzer will be activated periodically +#define WARN_BAT_LEVEL1 (LOW_BAT_LEVEL*10) + +// at this level, the buzzer will be activated permanently +#define WARN_BAT_LEVEL2 (CRITIC_BAT_LEVEL*10) + +#pragma message "Battery checker included!" + +void init_bat_checker(void) { + LED_INIT(BAT_CHECKER_LED); + LED_OFF(BAT_CHECKER_LED); +} + +void bat_checker_periodic(void) { + static uint8_t bat_low_counter = 0; + if(electrical.vsupply < WARN_BAT_LEVEL1) { + if(bat_low_counter) + bat_low_counter--; + } else { + bat_low_counter = BAT_CHECKER_DELAY * bat_checker_periodic_FREQ; + } + + if(!bat_low_counter) { + if(electrical.vsupply < WARN_BAT_LEVEL2) { + LED_ON(BAT_CHECKER_LED); + } else if(electrical.vsupply < WARN_BAT_LEVEL1) { + LED_TOGGLE(BAT_CHECKER_LED); + } + } else { + LED_OFF(BAT_CHECKER_LED); + } +} + diff --git a/sw/airborne/modules/battery_buzzer/battery_buzzer.h b/sw/airborne/modules/bat_checker/bat_checker.h similarity index 73% rename from sw/airborne/modules/battery_buzzer/battery_buzzer.h rename to sw/airborne/modules/bat_checker/bat_checker.h index b32f70fbb7..ac3d2b80f0 100644 --- a/sw/airborne/modules/battery_buzzer/battery_buzzer.h +++ b/sw/airborne/modules/bat_checker/bat_checker.h @@ -1,6 +1,7 @@ /* - * Copyright (C) 2010 Eric Parsonage - * Copyright (C) 2011 Piotr Esden-Tempski + * $Id$ + * + * Copyright (C) 2012 Thomas Kolb * * This file is part of paparazzi. * @@ -18,15 +19,13 @@ * 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 BAT_CHECKER_H +#define BAT_CHECKER_H -#ifndef BATTERY_BUZZER_H -#define BATTERY_BUZZER_H +void init_bat_checker(void); +void bat_checker_periodic(void); -#include "std.h" - -extern void battery_buzzer_init(void); -extern void battery_buzzer_periodic(void); - -#endif /* BATTERY_BUZZER_H */ +#endif // BAT_CHECKER_H diff --git a/sw/airborne/modules/battery_buzzer/battery_buzzer.c b/sw/airborne/modules/battery_buzzer/battery_buzzer.c deleted file mode 100644 index 78ca632653..0000000000 --- a/sw/airborne/modules/battery_buzzer/battery_buzzer.c +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright (C) 2010 Eric Parsonage - * Copyright (C) 2011 Piotr Esden-Tempski - * - * 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 battery_buzzer.c - * Simple module to toggle a gpio with connected buzzer on Lisa/M, - * when battery voltage drops below a certain level. - */ - -#include -#include -#include "battery_buzzer.h" -#include "generated/airframe.h" -#include "subsystems/electrical.h" - -/* initialises GPIO pins */ -void battery_buzzer_init(void) { - /* initialise peripheral clock for port C */ - RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC, ENABLE); - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOC, &GPIO_InitStructure); - /* set port C pin 10 to be low */ - GPIO_WriteBit(GPIOC, GPIO_Pin_10, Bit_SET); - - battery_buzzer_periodic(); -} - -/* sets GPIO pins */ -void battery_buzzer_periodic(void) { - - if (electrical.vsupply < (LOW_BAT_LEVEL * 10)) { - GPIO_WriteBit(GPIOC, GPIO_Pin_10 , Bit_RESET); - } else { - GPIO_WriteBit(GPIOC, GPIO_Pin_10 , Bit_SET); - } -} diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 4aeabadb3b..9306fbe328 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -141,32 +141,33 @@ void ins_realign_v(float z) { void ins_propagate() { /* untilt accels */ - struct Int32Vect3 accel_body; - INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel); - struct Int32Vect3 accel_ltp; - INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body); - float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z); + struct Int32Vect3 accel_meas_body; + INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32Vect3 accel_meas_ltp; + INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body); #if USE_VFF + float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); if (baro.status == BS_RUNNING && ins_baro_initialised) { - vff_propagate(z_accel_float); + vff_propagate(z_accel_meas_float); ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { // feed accel from the sensors - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float); + // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } #else - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float); + ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); #endif /* USE_VFF */ #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); #else - ins_ltp_accel.x = accel_ltp.x; - ins_ltp_accel.y = accel_ltp.y; + ins_ltp_accel.x = accel_meas_ltp.x; + ins_ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);