Merge branch 'dev' into dev_i2c

conf/airframes/CDW/debug_i2c.xml

@@ -213,7 +213,6 @@
   <firmware name="rotorcraft">
 <!--     <define name="USE_PERSISTENT_SETTINGS"/> -->
 <!--     <define name="USE_INS_NAV_INIT"/> -->
-     <define name="USE_ADAPT_HOVER"/>
      <define name="FAILSAFE_GROUND_DETECT"/>
      <define name="USE_GPS_ACC4R"/>
      <target name="ap" board="lisa_l_1.0">

conf/airframes/ENAC/quadrotor/blender.xml

@@ -11,7 +11,6 @@
 
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
-    <define name="USE_ADAPT_HOVER"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
     <target name="ap" board="booz_1.0">

conf/airframes/ENAC/quadrotor/booz2_g1.xml

@@ -15,7 +15,6 @@
 
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
-    <define name="USE_ADAPT_HOVER"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
     <target name="ap" board="booz_1.0">

conf/airframes/ENAC/quadrotor/navgo.xml

@@ -10,7 +10,6 @@
 
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
-    <define name="USE_ADAPT_HOVER"/>
     <define name="USE_ATTITUDE_REF" value="0"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
@@ -105,7 +104,6 @@
 
   <!-- Magnetic field for Toulouse (declination -16°) -->
   <section name="AHRS" prefix="AHRS_">
-    <define name="MAG_UPDATE_YAW_ONLY" value="1"/>
     <define name="PROPAGATE_FREQUENCY" value="512"/>
     <define name="H_X" value=" 0.513081"/>
     <define name="H_Y" value="-0.00242783"/>

conf/airframes/NoVa_L.xml

@@ -242,7 +242,6 @@
   <firmware name="rotorcraft">
 <!--     <define name="USE_PERSISTENT_SETTINGS"/> -->
 <!--     <define name="USE_INS_NAV_INIT"/> -->
-     <define name="USE_ADAPT_HOVER"/>
      <define name="FAILSAFE_GROUND_DETECT"/>
      <define name="USE_GPS_ACC4R"/>
      <target name="ap" board="lisa_l_1.0">

conf/airframes/Poine/booz2_a7.xml

@@ -214,7 +214,6 @@
   <firmware name="rotorcraft">
 <!--     <define name="USE_PERSISTENT_SETTINGS"/> -->
 <!--     <define name="USE_INS_NAV_INIT"/> -->
-     <define name="USE_ADAPT_HOVER"/>
      <define name="FAILSAFE_GROUND_DETECT"/>
      <define name="USE_GPS_ACC4R"/>
      <target name="ap" board="lisa_l_1.0">

conf/airframes/fraser_lisa_m_rotorcraft.xml

@@ -16,6 +16,7 @@
       <define name="USE_INS_NAV_INIT"/>
       <configure name="AHRS_ALIGNER_LED" value="3"/>
       <configure name="USE_NEW_I2C_DRIVER" value="1"/>
+      <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING"/>
     </target>
     <target name="sim" board="pc">
       <subsystem name="fdm"           type="nps"/>
@@ -108,39 +109,38 @@
 
   <section name="AHRS" prefix="AHRS_">
     <define name="PROPAGATE_FREQUENCY" value="512"/>
-    <define name="MAG_UPDATE_YAW_ONLY"/>
     <define name="H_X" value="0.3770441"/>
     <define name="H_Y" value="0.0193986"/>
     <define name="H_Z" value="0.9259921"/>
   </section>
 
   <section name="INS" prefix="INS_">
-    <define name="BARO_SENS" value="3.3" integer="16"/>
+    <define name="BARO_SENS" value="22.3" integer="16"/>
   </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
     <!-- setpoints -->
-    <define name="SP_MAX_P" value="10000" />
+    <define name="SP_MAX_P" value="10000"/>
-    <define name="SP_MAX_Q" value="10000" />
+    <define name="SP_MAX_Q" value="10000"/>
-    <define name="SP_MAX_R" value="10000" />
+    <define name="SP_MAX_R" value="10000"/>
-    <define name="DEADBAND_P" value="0" />
+    <define name="DEADBAND_P" value="0"/>
-    <define name="DEADBAND_Q" value="0" />
+    <define name="DEADBAND_Q" value="0"/>
-    <define name="DEADBAND_R" value="200" />
+    <define name="DEADBAND_R" value="200"/>
-    <define name="REF_TAU" value="4" />
+    <define name="REF_TAU" value="4"/>
 
     <!-- feedback -->
-    <define name="GAIN_P" value="400" />
+    <define name="GAIN_P" value="400"/>
-    <define name="GAIN_Q" value="400" />
+    <define name="GAIN_Q" value="400"/>
-    <define name="GAIN_R" value="350" />
+    <define name="GAIN_R" value="350"/>
 
-    <define name="IGAIN_P" value="75" />
+    <define name="IGAIN_P" value="75"/>
-    <define name="IGAIN_Q" value="75" />
+    <define name="IGAIN_Q" value="75"/>
-    <define name="IGAIN_R" value="50" />
+    <define name="IGAIN_R" value="50"/>
 
     <!-- feedforward -->
-    <define name="DDGAIN_P" value="300" />
+    <define name="DDGAIN_P" value="300"/>
-    <define name="DDGAIN_Q" value="300" />
+    <define name="DDGAIN_Q" value="300"/>
-    <define name="DDGAIN_R" value="300" />
+    <define name="DDGAIN_R" value="300"/>
   </section>
 
 
@@ -204,7 +204,7 @@
     <define name="RC_CLIMB_COEF" value ="163"/>
     <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
     <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
-    <!--define name="INV_M" value ="0.21"/-->
+    <!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
   </section>
 
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">

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

conf/modules/bat_checker.xml

@@ -0,0 +1,21 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<!--
+     Battery checker module
+     @define BAT_CHECKER_DELAY Number of seconds the battery voltage has to be
+                               below LOW_BAT_LEVEL before the warning signal is
+                               activated.
+     @define BAT_CHECKER_LED   The LED to use for the warning signal.
+     -->
+<module name="bat_checker">
+  <header>
+    <file name="bat_checker.h"/>
+  </header>
+  <init fun="init_bat_checker()"/>
+  <periodic fun="bat_checker_periodic()" freq="2" autorun="TRUE"/>
+  <makefile>
+    <define name="bat_checker_periodic_FREQ" value="2"/>
+    <file name="bat_checker.c"/>
+  </makefile>
+</module>
+

conf/modules/battery_buzzer.xml

@@ -1,19 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="battery_buzzer">
-  <header>
-    <file name="battery_buzzer.h"/>
-  </header>
-  <init fun="battery_buzzer_init()"/>
-  <periodic fun="battery_buzzer_periodic()" freq="5."/>
-  <makefile target="ap">
-    <file name="battery_buzzer.c"/>
-    <raw>
-ifneq ($(ARCH), stm32)
-$(error The battery_buzzer module is currently only implemented for STM32 based boards.)
-endif
-    </raw>
-  </makefile>
-</module>
-
-

conf/settings/settings_booz2.xml

@@ -43,6 +43,7 @@
       <dl_setting var="guidance_v_kd" min="0" step="1" max="600"   module="guidance/guidance_v" shortname="kd"/>
       <dl_setting var="guidance_v_ki" min="0" step="1" max="300"   module="guidance/guidance_v" shortname="ki" handler="SetKi" />
       <dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
+      <dl_setting var="guidance_v_nominal_throttle" min="0.1" step="0.01" max="0.9" module="guidance/guidance_v" shortname="nominal_throttle" handler="SetNominalHoverThrottle" alt_unit="%" alt_unit_coef="0.000104167"/>
       <dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
    </dl_settings>
 

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
+   ADC_1 PC3/ADC13
-   ADC2 PA0/ADC0
+   ADC_2 PC0/ADC10
-   ADC3 PC0/ADC10
+   ADC_3 PC1/ADC11
-   ADC4 PC1/ADC11
+   ADC_4 PC5/ADC15
-   ADC5 PC5/ADC15
+   ADC_6 PC2/ADC12
-   ADC6 PA1/ADC1
+   BATT  PC4/ADC14
-   ADC7 PC2/ADC12
-   BATT PC4/ADC14
 */
 #define BOARD_ADC_CHANNEL_1 ADC_Channel_13
-#define BOARD_ADC_CHANNEL_2 ADC_Channel_0
+#define BOARD_ADC_CHANNEL_2 ADC_Channel_10
-// FIXME - removed for now and used for battery monitoring
+#define BOARD_ADC_CHANNEL_3 ADC_Channel_11
-//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10
+// we can only use ADC1,2,3; the last channel is for bat monitoring
-#define BOARD_ADC_CHANNEL_3 ADC_Channel_14
+#define BOARD_ADC_CHANNEL_4 ADC_Channel_14
-#define BOARD_ADC_CHANNEL_4 ADC_Channel_11
+
+/* 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
 

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 */

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -1,7 +1,5 @@
 /*
- * $Id$
+ * Copyright (C) 2008-2012 The Paparazzi Team
- *
- * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
  *
  * 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)
+#ifndef AUTOPILOT_DISABLE_AHRS_KILL
-// Motors ON check state machine
+#include "subsystems/ahrs.h"
-#define STATUS_MOTORS_OFF           0
+static inline int ahrs_is_aligned(void) {
-#define STATUS_M_OFF_STICK_PUSHED   1
+  return (ahrs.status == AHRS_RUNNING);
-#define STATUS_START_MOTORS         2
+}
-#define STATUS_MOTORS_ON            3
+#else
-#define STATUS_M_ON_STICK_PUSHED    4
+static inline int ahrs_is_aligned(void) {
-#define STATUS_STOP_MOTORS          5
+  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;
+  autopilot_arming_set(autopilot_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;
-  };
 }
 
 
@@ -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 (kill_switch_is_on())
-  if (radio_control.values[RADIO_KILL_SWITCH] < 0)
     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
+  // an arming sequence is used to start/stop motors
-#ifndef RADIO_KILL_SWITCH
+  autopilot_arming_check_motors_on();
-  // no arming sequence and no kill switch
+
-  // motors are turned on when "in_flight" is detected
+  kill_throttle = ! autopilot_motors_on;
-  // 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();
   autopilot_check_in_flight(autopilot_motors_on);
-#endif
-  kill_throttle = !autopilot_motors_on;
 
   if (autopilot_mode > AP_MODE_FAILSAFE) {
     guidance_v_read_rc();

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
  *
  * 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_motors_on;
-extern bool_t  autopilot_in_flight;
+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) {                            \
+#define autopilot_KillThrottle(_v) { \
-    kill_throttle = _v;                                         \
+    autopilot_set_motors_on(_v);     \
-    if (kill_throttle) autopilot_motors_on = FALSE;				\
-    else autopilot_motors_on = TRUE;                            \
   }
 
 #ifdef POWER_SWITCH_LED

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 */

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 */

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 */

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 */

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
+    guidance_v_z_sp = ins_ltp_pos.z; // for display only
-    GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ?
     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
+#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
-  const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, GV_ADAPT_X_FRAC);
+  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_fb_cmd = ((-guidance_v_kp * err_z)  >> 12) +
-                      ((-guidance_v_kd * GUIDANCE_V_GAIN_SCALER * err_zd) >> 21) +
+                      ((-guidance_v_kd * err_zd) >> 21) +
-                      ((-guidance_v_ki * GUIDANCE_V_GAIN_SCALER * guidance_v_z_sum_err) >> 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);

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 */

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);
+  }
+}
+

sw/airborne/modules/bat_checker/bat_checker.h

@@ -1,6 +1,7 @@
 /*
- * Copyright (C) 2010 Eric Parsonage
+ * $Id$
- * Copyright (C) 2011 Piotr Esden-Tempski <piotr@esden.net>
+ *
+ * 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
+void init_bat_checker(void);
-#define BATTERY_BUZZER_H
+void bat_checker_periodic(void);
 
-#include "std.h"
+#endif // BAT_CHECKER_H
-
-extern void battery_buzzer_init(void);
-extern void battery_buzzer_periodic(void);
-
-#endif  /* BATTERY_BUZZER_H */

sw/airborne/modules/battery_buzzer/battery_buzzer.c

@@ -1,57 +0,0 @@
-/*
- * Copyright (C) 2010 Eric Parsonage
- * Copyright (C) 2011 Piotr Esden-Tempski <piotr@esden.net>
- *
- * 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 <stm32/gpio.h>
-#include <stm32/rcc.h>
-#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);
-  }
-}

sw/airborne/subsystems/ins.c

@@ -141,32 +141,33 @@ void ins_realign_v(float z) {
 
 void ins_propagate() {
   /* untilt accels */
-  struct Int32Vect3 accel_body;
+  struct Int32Vect3 accel_meas_body;
-  INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
+  INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
-  struct Int32Vect3 accel_ltp;
+  struct Int32Vect3 accel_meas_ltp;
-  INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
+  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
-  float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
 
 #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.x = accel_meas_ltp.x;
-  ins_ltp_accel.y = accel_ltp.y;
+  ins_ltp_accel.y = accel_meas_ltp.y;
 #endif /* USE_HFF */
 
   INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);