Merge branch 'dev' into 4.0_beta

* utm for skytraq gps
* rotorcraft vertical guidance adaption improvements (should be usable with pwm_supervision now)

conf/airframes/CDW/LisaAspirin2.xml

@@ -198,7 +198,7 @@
 
   <firmware name="fixedwing">
 
-    <target name="ap" board="lisa_m_1.0">
+    <target name="ap" board="lisa_l_1.0">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
@@ -206,7 +206,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
-       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" /> 
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="1"/>
@@ -232,7 +232,7 @@
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Communication -->
-    <subsystem name="telemetry" type="transparent">
+    <subsystem name="telemetry" type="xbee_api">
       <configure name="MODEM_BAUD" value="B57600"/>
     </subsystem>
 
@@ -240,7 +240,7 @@
     <subsystem name="control"/>
     <!-- Sensors -->
     <subsystem name="navigation"/>
-    <subsystem name="gps" type="ublox_utm"/>
+    <subsystem name="gps" type="ublox"/>
 
 
 </firmware>

conf/airframes/booz2_flixr.xml

@@ -37,7 +37,9 @@
     <load name="booz_drop.xml"/>
     <load name="sys_mon.xml"/>
     <!--load name="booz_cam.xml"/-->
-    <!--load name="sonar_maxbotix_booz.xml"/-->
+    <load name="sonar_maxbotix_booz.xml">
+      <configure name="ADC_SONAR" value="ADC_0"/>
+    </load>
   </modules>
 
   <commands>
@@ -185,7 +187,8 @@
   </section>
 
   <section name="INS" prefix="INS_">
-    <define name="BARO_SENS" value="15." integer="16" />
+    <define name="BARO_SENS" value="15." integer="16"/>
+    <define name="SONAR_SENS" value="2.146" integer="16"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">

conf/autopilot/subsystems/fixedwing/gps_skytraq.makefile

@@ -0,0 +1,22 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
+ap.CFLAGS += -DUSE_$(GPS_PORT)
+ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+
+ifneq ($(GPS_LED),none)
+  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.

conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile

@@ -1,14 +1,18 @@
+# Hey Emacs, this is a -*- makefile -*-
 
-ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.CFLAGS += -DUSE_GPS
-ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
-ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
+ap.CFLAGS += -DUSE_$(GPS_PORT)
-
+ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
-ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
-ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT)
 
 ifneq ($(GPS_LED),none)
   ap.CFLAGS += -DGPS_LED=$(GPS_LED)
 endif
 
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
+
+
 sim.CFLAGS += -DUSE_GPS
 sim.srcs += $(SRC_SUBSYSTEMS)/gps.c

conf/boards/lisa_m_1.0.makefile

@@ -1,7 +1,8 @@
+# Hey Emacs, this is a -*- makefile -*-
 #
 # lisa_m_1.0.makefile
 #
-# http://paparazzi.enac.fr/wiki/User/LisaM
+# http://paparazzi.enac.fr/wiki/Lisa/M
 #
 
 BOARD=lisa_m
@@ -51,9 +52,13 @@ ifndef SYS_TIME_LED
 SYS_TIME_LED = 1
 endif
 
-
+ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT   = UART3
+endif
+
+ifndef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
 RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT = UART5
+endif
 
 ifndef MODEM_PORT
 MODEM_PORT=UART2

conf/boards/lisa_m_2.0.makefile

@@ -1,14 +1,12 @@
 # Hey Emacs, this is a -*- makefile -*-
 
-include $(PAPARAZZI_SRC)/conf/boards/lisa_m_1.0.makefile
-
 #
 # Swap GPS UART with spektrum UART
 #
 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT   = UART1
-GPS_PORT=UART3
 
-#
+ifndef GPS_PORT
-# Disable aligner led for now.
+GPS_PORT=UART3
-#
+endif
-AHRS_ALIGNER_LED = none
+
+include $(PAPARAZZI_SRC)/conf/boards/lisa_m_1.0.makefile

conf/messages.xml

@@ -1792,7 +1792,7 @@
     <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV"/>
     <field name="ap_v_mode"    type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV"/>
     <field name="vsupply"      type="uint8" unit="decivolt"/>
-    <field name="cpu_time"     type="uint16" unit="s"></field>
+    <field name="cpu_time"     type="uint16" unit="s"/>
   </message>
 
    <message name="STATE_FILTER_STATUS" id="232">

conf/modules/demo_module.xml

@@ -9,7 +9,7 @@
   <periodic fun="periodic_10Hz_demo()" period="0.1" start="start_demo()" stop="stop_demo()" autorun="FALSE"/>
   <makefile>
     <raw>
-#Exemple of RAW makefile part
+#Example of RAW makefile part
     </raw>
     <define name="DEMO_MODULE_LED" value="2"/>
     <file name="demo_module.c"/>

conf/modules/sonar_maxbotix_booz.xml

@@ -19,6 +19,12 @@
     <file name="sonar_maxbotix.c"/>
   </makefile>
   <makefile target="ap">
+    <raw>
+# set ADC_SONAR to ADC_0 as default
+ifeq ($(ADC_SONAR),)
+ADC_SONAR = ADC_0
+endif
+    </raw>
     <define name="ADC_CHANNEL_SONAR" value="$(ADC_SONAR)"/>
     <define name="USE_$(ADC_SONAR)"/>
     <define name="USE_SONAR"/>

conf/settings/tuning_ctl_new.xml

@@ -42,6 +42,8 @@
           <strip_button name="AS" value="1" group="speed_mode"/>
           <strip_button name="GS" value="2" group="speed_mode"/>
         </dl_setting>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_min_cruise_throttle" shortname="min cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE"/>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_max_cruise_throttle" shortname="max cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE"/>
         <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="guidance/guidance_v" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
 
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>

conf/settings/tuning_ins.xml

@@ -55,7 +55,7 @@
       </dl_settings>
 
       <dl_settings name="alt">
-        <dl_setting MAX="0.2" MIN="2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
+        <dl_setting MAX="0.2" MIN="0" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN" module="guidance/guidance_v"/>
         <dl_setting MAX="10" MIN="0.1" STEP="0.1" VAR="v_ctl_altitude_max_climb" shortname="max climb" param="V_CTL_ALTITUDE_MAX_CLIMB"/>
         <dl_setting MAX="2" MIN="0.0" STEP="0.05" VAR="v_ctl_altitude_pre_climb_correction" shortname="pre climb cor" param="V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION"/>
       </dl_settings>

sw/airborne/arch/lpc21/peripherals/ms2100_arch.c

@@ -17,11 +17,11 @@ void ms2100_arch_init( void ) {
   ms2100_cur_axe = 0;
 
   /* configure SS pin */
-  Ms2001Unselect();                       /* pin idles high */
+  Ms2100Unselect();                       /* pin idles high */
   SetBit(MS2100_SS_IODIR, MS2100_SS_PIN); /* pin is output  */
 
   /* configure RESET pin */
-  Ms2001Reset();                                /* pin idles low  */
+  Ms2100Reset();                                /* pin idles low  */
   SetBit(MS2100_RESET_IODIR, MS2100_RESET_PIN); /* pin is output  */
 
   /* configure DRDY pin */

sw/airborne/arch/lpc21/peripherals/ms2100_arch.h

@@ -16,20 +16,20 @@
 
 extern volatile uint8_t ms2100_cur_axe;
 
-#define Ms2001Select()   SetBit(MS2100_SS_IOCLR,MS2100_SS_PIN)
+#define Ms2100Select()   SetBit(MS2100_SS_IOCLR,MS2100_SS_PIN)
-#define Ms2001Unselect() SetBit(MS2100_SS_IOSET,MS2100_SS_PIN)
+#define Ms2100Unselect() SetBit(MS2100_SS_IOSET,MS2100_SS_PIN)
 
-#define Ms2001Reset() SetBit(MS2100_RESET_IOCLR,MS2100_RESET_PIN)
+#define Ms2100Reset() SetBit(MS2100_RESET_IOCLR,MS2100_RESET_PIN)
-#define Ms2001Set()   SetBit(MS2100_RESET_IOSET,MS2100_RESET_PIN)
+#define Ms2100Set()   SetBit(MS2100_RESET_IOSET,MS2100_RESET_PIN)
 
-#define Ms2001OnSpiInt() {                                   \
+#define Ms2100OnSpiInt() {                                   \
     switch (ms2100_status) {                                \
     case MS2100_SENDING_REQ:                                \
       {                                                     \
         /* read dummy control byte reply */                 \
         uint8_t foo __attribute__ ((unused)) = SSPDR;       \
         ms2100_status = MS2100_WAITING_EOC;                 \
-        Ms2001Unselect();                                   \
+        Ms2100Unselect();                                   \
         SSP_ClearRti();                                     \
         SSP_DisableRti();                                   \
         SSP_Disable();                                      \
@@ -42,7 +42,7 @@ extern volatile uint8_t ms2100_cur_axe;
         new_val += SSPDR;                                   \
         if (abs(new_val) < 2000)                            \
           ms2100_values[ms2100_cur_axe] = new_val;			\
-        Ms2001Unselect();                                   \
+        Ms2100Unselect();                                   \
         SSP_ClearRti();                                     \
         SSP_DisableRti();                                   \
         SSP_Disable();                                      \
@@ -59,22 +59,22 @@ extern volatile uint8_t ms2100_cur_axe;
   }
 
 
-#define Ms2001SendReq() {                               \
+#define Ms2100SendReq() {                               \
-    Ms2001Select();                                     \
+    Ms2100Select();                                     \
     ms2100_status = MS2100_SENDING_REQ;					\
-    Ms2001Set();                                        \
+    Ms2100Set();                                        \
     SSP_ClearRti();                                     \
     SSP_EnableRti();                                    \
-    Ms2001Reset();                                      \
+    Ms2100Reset();                                      \
     uint8_t control_byte = (ms2100_cur_axe+1) << 0 |    \
       MS2100_DIVISOR << 4;                              \
     SSP_Send(control_byte);                             \
     SSP_Enable();                                       \
   }
 
-#define Ms2001ReadRes() {						\
+#define Ms2100ReadRes() {						\
     ms2100_status = MS2100_READING_RES;         \
-    Ms2001Select();                             \
+    Ms2100Select();                             \
     /* trigger 2 bytes read */                  \
     SSP_Send(0);                                \
     SSP_Send(0);                                \

sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c

@@ -115,10 +115,10 @@ static void SSP_ISR(void) {
     if (ms2100_status == MS2100_IDLE || ms2100_status == MS2100_GOT_EOC) {
       ImuSetSSP8bits();
       if (ms2100_status == MS2100_IDLE) {
-        Ms2001SendReq();
+        Ms2100SendReq();
       }
       else { /* MS2100_GOT_EOC */
-        Ms2001ReadRes();
+        Ms2100ReadRes();
       }
       imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
     }
@@ -127,9 +127,9 @@ static void SSP_ISR(void) {
     }
     break;
   case IMU_SSP_STA_BUSY_MS2100:
-    Ms2001OnSpiInt();
+    Ms2100OnSpiInt();
     if (ms2100_status == MS2100_IDLE) {
-      Ms2001SendReq();
+      Ms2100SendReq();
       imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
     }
     else

sw/airborne/arch/stm32/peripherals/ms2100_arch.c

@@ -41,8 +41,8 @@ void ms2100_arch_init( void ) {
   ms2100_cur_axe = 0;
 
   /* set mag SS and reset as output and assert them (SS on PC12  reset on PC13) ----*/
-  Ms2001Unselect();
+  Ms2100Unselect();
-  Ms2001Set();
+  Ms2100Set();
 
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
   GPIO_InitTypeDef GPIO_InitStructure;
@@ -83,13 +83,13 @@ void ms2100_arch_init( void ) {
 
 #ifdef MS2100_HANDLES_SPI_IRQ
 void spi2_irq_handler(void) {
-  Ms2001OnSpiIrq();
+  Ms2100OnSpiIrq();
 }
 #endif
 
 
 #ifdef MS2100_HANDLES_DMA_IRQ
 void dma1_c4_irq_handler(void) {
-  Ms2001OnDmaIrq();
+  Ms2100OnDmaIrq();
 }
 #endif /* MS2100_HANDLES_DMA_IRQ */

sw/airborne/arch/stm32/peripherals/ms2100_arch.h

@@ -30,21 +30,21 @@
 extern uint8_t ms2100_cur_axe;
 extern int16_t ms2100_last_reading;
 
-#define Ms2001Select()   GPIOC->BRR = GPIO_Pin_12
+#define Ms2100Select()   GPIOC->BRR = GPIO_Pin_12
-#define Ms2001Unselect() GPIOC->BSRR = GPIO_Pin_12
+#define Ms2100Unselect() GPIOC->BSRR = GPIO_Pin_12
 
-#define Ms2001Reset() GPIOC->BSRR = GPIO_Pin_13;
+#define Ms2100Reset() GPIOC->BSRR = GPIO_Pin_13;
-#define Ms2001Set()   GPIOC->BRR = GPIO_Pin_13
+#define Ms2100Set()   GPIOC->BRR = GPIO_Pin_13
 
-#define Ms2001HasEOC() GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)
+#define Ms2100HasEOC() GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)
 
-#define Ms2001SendReq() {						\
+#define Ms2100SendReq() {						\
-    Ms2001Select();							\
+    Ms2100Select();							\
     __IO uint32_t nCount = 4;for(; nCount != 0; nCount--);		\
-    Ms2001Reset();							\
+    Ms2100Reset();							\
     ms2100_status = MS2100_SENDING_REQ;					\
     nCount = 4;for(; nCount != 0; nCount--);				\
-    Ms2001Set();							\
+    Ms2100Set();							\
     uint16_t ctl_byte = ((ms2100_cur_axe+1) | (MS2100_DIVISOR << 4));	\
     nCount = 20;for(; nCount != 0; nCount--);				\
     SPI_Cmd(SPI2, DISABLE);						\
@@ -65,9 +65,9 @@ extern int16_t ms2100_last_reading;
     SPI_I2S_SendData(SPI2, ctl_byte);					\
   }
 
-#define Ms2001ReadRes() {						\
+#define Ms2100ReadRes() {						\
     ms2100_status = MS2100_READING_RES;					\
-    Ms2001Select();							\
+    Ms2100Select();							\
     SPI_Cmd(SPI2, DISABLE);						\
     SPI_InitTypeDef SPI_InitStructure = {				\
       .SPI_Direction = SPI_Direction_2Lines_FullDuplex,			\
@@ -122,13 +122,13 @@ extern int16_t ms2100_last_reading;
                                         \
   }
 
-#define Ms2001OnDmaIrq() {					\
+#define Ms2100OnDmaIrq() {					\
     /*  ASSERT((ms2100_status == MS2100_READING_RES),		\
      *   DEBUG_MS2100, MS2100_ERR_SPURIOUS_DMA_IRQ);		\
      */								\
     if (abs(ms2100_last_reading) < 1000)			\
       ms2100_values[ms2100_cur_axe] = ms2100_last_reading;	\
-    Ms2001Unselect();						\
+    Ms2100Unselect();						\
     ms2100_cur_axe++;						\
     if (ms2100_cur_axe > 2) {					\
       ms2100_cur_axe = 0;					\
@@ -140,13 +140,13 @@ extern int16_t ms2100_last_reading;
     DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);		\
   }
 
-#define Ms2001OnSpiIrq() {						\
+#define Ms2100OnSpiIrq() {						\
     /*  ASSERT((ms2100_status == MS2100_SENDING_REQ),			\
      *   DEBUG_MS2100, MS2100_ERR_SPURIOUS_SPI_IRQ);			\
      */									\
     /* read unused control byte reply */				\
     uint8_t foo __attribute__ ((unused)) = SPI_I2S_ReceiveData(SPI2);	\
-    Ms2001Unselect();							\
+    Ms2100Unselect();							\
     ms2100_status = MS2100_WAITING_EOC;					\
     SPI_Cmd(SPI2, DISABLE);						\
     SPI_I2S_ITConfig(SPI2, SPI_I2S_IT_RXNE, DISABLE);			\

sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c

@@ -93,11 +93,11 @@ void dma1_c4_irq_handler(void) {
     SPI_Cmd(SPI2, DISABLE);
 #if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
     if (ms2100_status == MS2100_IDLE) {
-      Ms2001SendReq();
+      Ms2100SendReq();
       imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
     }
-    else if (ms2100_status == MS2100_WAITING_EOC && Ms2001HasEOC()) {
+    else if (ms2100_status == MS2100_WAITING_EOC && Ms2100HasEOC()) {
-      Ms2001ReadRes();
+      Ms2100ReadRes();
       imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
     }
     else
@@ -106,7 +106,7 @@ void dma1_c4_irq_handler(void) {
     break;
   case IMU_SSP_STA_BUSY_MS2100:
 #if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
-    Ms2001OnDmaIrq();
+    Ms2100OnDmaIrq();
 #endif
     break;
   default:
@@ -118,6 +118,6 @@ void dma1_c4_irq_handler(void) {
 
 void spi2_irq_handler(void) {
 #if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
-  Ms2001OnSpiIrq();
+  Ms2100OnSpiIrq();
 #endif
 }

sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c

@@ -56,6 +56,8 @@ uint8_t v_ctl_auto_throttle_submode;
 /* "auto throttle" inner loop parameters */
 float v_ctl_auto_throttle_cruise_throttle;
 float v_ctl_auto_throttle_nominal_cruise_throttle;
+float v_ctl_auto_throttle_min_cruise_throttle;
+float v_ctl_auto_throttle_max_cruise_throttle;
 float v_ctl_auto_throttle_climb_throttle_increment;
 float v_ctl_auto_throttle_pgain;
 float v_ctl_auto_throttle_igain;
@@ -133,6 +135,8 @@ void v_ctl_init( void ) {
   /* "auto throttle" inner loop parameters */
   v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
   v_ctl_auto_throttle_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle;
+  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
+  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
   v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
   v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
   v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -37,6 +37,7 @@
 
 #include "generated/airframe.h"
 
+
 /* warn if some gains are still negative */
 #if (GUIDANCE_V_HOVER_KP < 0) ||                   \
   (GUIDANCE_V_HOVER_KD < 0)   ||                   \
@@ -44,42 +45,28 @@
 #warning "ALL control gains are now positive!!!"
 #endif
 
-/* In case Asctec controllers are used without supervision */
-#ifndef SUPERVISION_MIN_MOTOR
-#define SUPERVISION_MIN_MOTOR 1
-#endif
-#ifndef SUPERVISION_MAX_MOTOR
-#define SUPERVISION_MAX_MOTOR 200
-#endif
 
 uint8_t guidance_v_mode;
 int32_t guidance_v_ff_cmd;
 int32_t guidance_v_fb_cmd;
-/* command output                                 */
 int32_t guidance_v_delta_t;
 
-/* direct throttle from radio control             */
-/* range 0:200                                    */
-int32_t guidance_v_rc_delta_t;
-/* vertical speed setpoint from radio control     */
-/* Q12.19 : accuracy 0.0000019, range +/-4096     */
-int32_t guidance_v_rc_zd_sp;
-/* altitude setpoint in meter (input)             */
-/* Q23.8 : accuracy 0.0039, range 8388km          */
-int32_t guidance_v_z_sp;
-/* vertical speed setpoint in meter/s (input)     */
-/* Q12.19 : accuracy 0.0000019, range +/-4096     */
-int32_t guidance_v_zd_sp;
-#define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
 
-/* altitude reference in meter                    */
+/** Direct throttle from radio control.
-/* Q23.8 : accuracy 0.0039, range 8388km          */
+ *  range 0:200
+ */
+int32_t guidance_v_rc_delta_t;
+
+/** Vertical speed setpoint from radio control.
+ *  fixed point representation: Q12.19
+ *  accuracy 0.0000019, range +/-4096
+ */
+int32_t guidance_v_rc_zd_sp;
+
+int32_t guidance_v_z_sp;
+int32_t guidance_v_zd_sp;
 int32_t guidance_v_z_ref;
-/* vertical speed reference in meter/s            */
-/* Q12.19 : accuracy 0.0000038, range 4096        */
 int32_t guidance_v_zd_ref;
-/* vertical acceleration reference in meter/s^2   */
-/* Q21.10 : accuracy 0.0009766, range 2097152     */
 int32_t guidance_v_zdd_ref;
 
 int32_t guidance_v_kp;
@@ -117,14 +104,9 @@ void guidance_v_init(void) {
 void guidance_v_read_rc(void) {
 
   // used in RC_DIRECT directly and as saturation in CLIMB and HOVER
-#ifndef USE_HELI
+  guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
-  guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 200 / MAX_PPRZ;
-#else
-  guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 4 / 5;
-#endif
   // used in RC_CLIMB
-  guidance_v_rc_zd_sp   = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) *
+  guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF;
-                                GUIDANCE_V_RC_CLIMB_COEF;
   DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND);
 
 }
@@ -134,10 +116,6 @@ void guidance_v_mode_changed(uint8_t new_mode) {
   if (new_mode == guidance_v_mode)
     return;
 
-  //  switch ( guidance_v_mode ) {
-  //
-  //  }
-
   switch (new_mode) {
 
   case GUIDANCE_V_MODE_RC_CLIMB:
@@ -147,10 +125,11 @@ void guidance_v_mode_changed(uint8_t new_mode) {
     guidance_v_z_sum_err = 0;
     GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
     break;
+
   default:
     break;
-  }
 
+  }
 
   guidance_v_mode = new_mode;
 
@@ -167,14 +146,7 @@ void guidance_v_run(bool_t in_flight) {
   // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
   // AKA SUPERVISION and co
   if (in_flight) {
-    // we should use something after the supervision!!! fuck!!!
+    gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
-    int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
-    gv_adapt_run(ins_ltp_accel.z, cmd_hack);
-    //gv_adapt_run(ins_ltp_accel.z, cmd_hack, guidance_v_zd_ref);
-  }
-  else {
-    // reset vertical filter until takeoff
-    //ins_vf_realign = TRUE;
   }
 
   switch (guidance_v_mode) {
@@ -288,29 +260,23 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
 #endif
   const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
                           (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
-#if 0
+
-  if (g_m_zdd > 0)
-    guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m;
-  else
-    guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
-#else
   guidance_v_ff_cmd = g_m_zdd / inv_m;
   int32_t cphi,ctheta,cphitheta;
   PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
   PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
   cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
   if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
+  /* feed forward command */
   guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
-#endif
 
-  /* our error command                   */
+
+  /* our error feed back command                   */
   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);
 
-  // z-axis pointing down -> positive error means we need less thrust
+// z-axis pointing down -> positive error means we need less thrust
-  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_delta_t = -guidance_v_fb_cmd;
-
 
 }

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h

@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
  *
  * This file is part of paparazzi.
@@ -26,6 +24,17 @@
 
 #include "std.h"
 
+/** Supervision default bounds
+ *  In case Asctec controllers are used without supervision
+ *  Used in control and adaptation filter
+ * */
+#ifndef SUPERVISION_MIN_MOTOR
+#define SUPERVISION_MIN_MOTOR 1
+#endif
+#ifndef SUPERVISION_MAX_MOTOR
+#define SUPERVISION_MAX_MOTOR 200
+#endif
+
 #include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
 
@@ -38,19 +47,44 @@
 
 extern uint8_t guidance_v_mode;
 
+/** altitude setpoint in meters (input).
+ *  fixed point representation: Q23.8
+ *  accuracy 0.0039, range 8388km
+ */
 extern int32_t guidance_v_z_sp;
-extern int32_t guidance_v_zd_sp;
-extern int32_t guidance_v_z_ref;
-extern int32_t guidance_v_zd_ref;
-extern int32_t guidance_v_zdd_ref;
-extern int32_t guidance_v_z_sum_err;
-extern int32_t guidance_v_ff_cmd;
-extern int32_t guidance_v_fb_cmd;
-extern int32_t guidance_v_delta_t;
 
-extern int32_t guidance_v_kp;
+/** vertical speed setpoint in meter/s (input).
-extern int32_t guidance_v_kd;
+ *  fixed point representation: Q12.19
-extern int32_t guidance_v_ki;
+ *  accuracy 0.0000019, range +/-4096
+ */
+extern int32_t guidance_v_zd_sp;
+
+/** altitude reference in meters.
+ *  fixed point representation: Q23.8
+ *  accuracy 0.0039, range 8388km
+ */
+extern int32_t guidance_v_z_ref;
+
+/** vertical speed reference in meter/s.
+ *  fixed point representation: Q12.19
+ *  accuracy 0.0000038, range 4096
+ */
+extern int32_t guidance_v_zd_ref;
+
+/** vertical acceleration reference in meter/s^2.
+ *  fixed point representation: Q21.10
+ *  accuracy 0.0009766, range 2097152
+ */
+extern int32_t guidance_v_zdd_ref;
+
+extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain
+extern int32_t guidance_v_ff_cmd;    ///< feed-forward command
+extern int32_t guidance_v_fb_cmd;    ///< feed-back command
+extern int32_t guidance_v_delta_t;   ///< command output (ff+fb)
+
+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
 
 extern void guidance_v_init(void);
 extern void guidance_v_read_rc(void);

sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h

@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2009-2010 The Paparazzi Team
  *
  * This file is part of paparazzi.
@@ -19,37 +17,67 @@
  * 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 guidance_v_adapt.h
+ *  Adaptation bloc of the vertical guidance.
  *
- *
+ *  This is a dimension one kalman filter estimating
- * Adaptation bloc of the vertical guidance
+ *  the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
- * This is a dimension one kalman filter estimating
+ *  needed by the invert dynamic model to produce a nominal command
- * the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
- * needed by the invert dynamic model to produce a nominal command
  */
 
 #ifndef GUIDANCE_V_ADPT
 #define GUIDANCE_V_ADPT
 
+/** Adapt noise factor.
+ *  Smaller values will make the filter to adapter faster
+ *  Bigger values (slower adaptation) make the filter more robust to external perturbations
+ *  Factor should always be >0
+ */
+#ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
+#define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
+#endif
+
+/** Filter is not fed if accel values are more than +/- MAX_ACCEL
+ *  MAX_ACCEL is a positive value in m/s^2
+ */
+#ifndef GUIDANCE_V_ADAPT_MAX_ACCEL
+#define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0
+#endif
+
+/** Filter is not fed if command values are out of a % of MIN/MAX_SUPERVISION
+ *  MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD
+ */
+#ifndef GUIDANCE_V_ADAPT_MAX_CMD
+#define GUIDANCE_V_ADAPT_MAX_CMD 0.9
+#endif
+#ifndef GUIDANCE_V_ADAPT_MIN_CMD
+#define GUIDANCE_V_ADAPT_MIN_CMD 0.1
+#endif
+
+/** State of the estimator.
+ *  fixed point representation with #GV_ADAPT_X_FRAC
+ *  Q13.18
+ */
 extern int32_t gv_adapt_X;
+#define GV_ADAPT_X_FRAC 18
+
+/** Covariance.
+ *  fixed point representation with #GV_ADAPT_P_FRAC
+ *  Q13.18
+ */
 extern int32_t gv_adapt_P;
+#define GV_ADAPT_P_FRAC 18
+
+/** Measurement */
 extern int32_t gv_adapt_Xmeas;
 
 
 #ifdef GUIDANCE_V_C
 
-/* Our State
-   Q13.18
-*/
 int32_t gv_adapt_X;
-#define GV_ADAPT_X_FRAC 18
-
-/* Our covariance
-   Q13.18
-*/
 int32_t gv_adapt_P;
-#define GV_ADAPT_P_FRAC 18
-
-/* Our measurement */
 int32_t gv_adapt_Xmeas;
 
 
@@ -59,87 +87,97 @@ int32_t gv_adapt_Xmeas;
 #define GV_ADAPT_P0_F 0.5
 #define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
 
-/* System and Measuremement noises */
+/* System  noises */
 #define GV_ADAPT_SYS_NOISE_F 0.00005
 #define GV_ADAPT_SYS_NOISE  BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
 
-#if !USE_ADAPT_HOVER
+/* Measuremement noises */
-
+#define GV_ADAPT_MEAS_NOISE_HOVER_F (8.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
-#define GV_ADAPT_MEAS_NOISE_F 2.0
-#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
-
-#else /* USE_ADAPT_HOVER */
-
-#define GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
 #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
-#define GV_ADAPT_MEAS_NOISE_F 50.0
+#define GV_ADAPT_MEAS_NOISE_OF_ZD (20.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
-#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
 
-#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
+/* Max accel */
-#define GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
+#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
-#define GV_ADAPT_MAX_CMD 180
+
-#define GV_ADAPT_MIN_CMD 20
+/* Command bounds */
-#define GV_ADAPT_HOVER_MAX_CMD 120
+#define GV_ADAPT_MAX_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MAX_CMD))
-#define GV_ADAPT_HOVER_MIN_CMD 60
+#define GV_ADAPT_MIN_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MIN_CMD))
+
+/* Output bounds.
+ * Don't let it climb over a value that would
+ * give less than zero throttle and don't let it down to zero.
+ * Worst cases:
+ *   MIN_ACCEL / MAX_THROTTLE
+ *   MAX_ACCEL / MIN_THROTTLE
+ * ex:
+ *   9.81*2^18/255 = 10084
+ *   9.81*2^18/1 = 2571632
+ */
+// TODO Check this properly
+#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MIN_MOTOR)
+#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MAX_MOTOR)
 
-#endif /* USE_ADAPT_HOVER */
 
 static inline void gv_adapt_init(void) {
   gv_adapt_X = GV_ADAPT_X0;
   gv_adapt_P = GV_ADAPT_P0;
 }
 
-/*
-  zdd_meas : INT32_ACCEL_FRAC
-  thrust_applied : controller input [2-200]
-*/
 #define K_FRAC 12
-static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
 
-  /* Do you really think we want to divide by zero ? */
+/** Adaptation function.
-  if (thrust_applied == 0) return;
+ * @param zdd_meas        vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC
+ * @param thrust_applied  controller input [SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR]
+ * @param zd_ref          vertical speed reference in m/s with #INT32_SPEED_FRAC
+ */
+static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
+
+  /* Do you really think we want to divide by zero ?
+   * Negative commands are also prohibited here
+   */
+  if (thrust_applied <= 0) return;
+
   /* We don't propagate state, it's constant !       */
   /* We propagate our covariance                     */
   gv_adapt_P =  gv_adapt_P + GV_ADAPT_SYS_NOISE;
-  /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
+
-  const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
-#if USE_ADAPT_HOVER
   /* Update only if accel and commands are in a valid range */
   if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
-      || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL)
+      || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
     return;
-#endif
+  }
-  if ( g_m_zdd > 0)
+
+  /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
+  const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
+  if ( g_m_zdd > 0) {
     gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
-  else
+  } else {
     gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied;
+  }
+
   /* Compute a residual */
   int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
+
   /* Covariance Error   */
-  int32_t E = 0;
+  int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC);
-#if USE_ADAPT_HOVER
+  if (zd_ref < 0) ref = -ref;
-  if ((thrust_applied > GV_ADAPT_HOVER_MIN_CMD && thrust_applied < GV_ADAPT_HOVER_MAX_CMD) ||
+  int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD;
-      (zdd_meas > -GV_ADAPT_HOVER_ACCEL && zdd_meas < GV_ADAPT_HOVER_ACCEL))
+
-    E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER;
-  else
-#endif
-    E = gv_adapt_P + GV_ADAPT_MEAS_NOISE;
   /* Kalman gain        */
   int32_t K = (gv_adapt_P<<K_FRAC) / E;
+
   /* Update Covariance */
   gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
   /* Don't let covariance climb over initial value */
   if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0;
+
   /* Update State */
   gv_adapt_X = gv_adapt_X + ((K * residual)>>K_FRAC);
+
   /* Again don't let it climb over a value that would
-     give less than zero throttle and don't let it down to zero.
+   * give less than zero throttle and don't let it down to zero.
-     30254 = MAX_ACCEL*GV_ADAPT_X_FRAC/MAX_THROTTLE
+   */
-     aka
+  Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT);
-     30254 = 3*9.81*2^8/255
-     2571632 = 9.81*2^18
-  */
-  Bound(gv_adapt_X, 10000, 2571632);
 }
 
 

sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h

@@ -1,6 +1,4 @@
 /*
- * $Id$
- *
  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
  *
  * This file is part of paparazzi.
@@ -29,24 +27,35 @@
 #include "math/pprz_algebra.h"
 #include "math/pprz_algebra_int.h"
 
-/* update frequency                               */
+/* update frequency  */
 #define GV_FREQ_FRAC 9
 #define GV_FREQ (1<<GV_FREQ_FRAC)
 
-/* reference model vaccel in meters/sec2 (output) */
+/** reference model vertical accel in meters/s^2 (output)
-/* Q23.8 : accuracy 0.0039 , range 8388km/s2      */
+ *  fixed point representation with #GV_ZDD_REF_FRAC
-/* int32_4_8_t */
+ *  Q23.8 : accuracy 0.0039 , range 8388km/s^2
+ */
 extern int32_t gv_zdd_ref;
+
+/** number of bits for the fractional part of #gv_zdd_ref */
 #define GV_ZDD_REF_FRAC 8
 
-/* reference model vspeed in meters/sec (output)  */
+/** reference model vertical speed in meters/sec (output)
-/* Q14.17 : accuracy 0.0000076 , range 16384m/s2  */
+ *  fixed point representation with #GV_ZD_REF_FRAC
+ *  Q14.17 : accuracy 0.0000076 , range 16384m/s2
+ */
 extern int32_t gv_zd_ref;
+
+/** number of bits for the fractional part of #gv_zd_ref */
 #define GV_ZD_REF_FRAC (GV_ZDD_REF_FRAC + GV_FREQ_FRAC)
 
-/* reference model altitude in meters (output)    */
+/** reference model altitude in meters (output)
-/* Q37.26 :                                       */
+ *  fixed point representation with #GV_Z_REF_FRAC
+ *  Q37.26 :
+ */
 extern int64_t gv_z_ref;
+
+/** number of bits for the fractional part of #gv_z_ref */
 #define GV_Z_REF_FRAC (GV_ZD_REF_FRAC + GV_FREQ_FRAC)
 
 /* Saturations definition */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c

@@ -32,6 +32,9 @@
 #define REF_DOT_FRAC 11
 #define REF_FRAC  16
 
+#ifndef SUPERVISION_SCALE
+#define SUPERVISION_SCALE MAX_PPRZ
+#endif
 
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))

sw/airborne/firmwares/rotorcraft/telemetry.h

@@ -57,6 +57,7 @@
 #define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) {			\
     uint32_t imu_nb_err = 0;						\
     uint8_t _twi_blmc_nb_err = 0;					\
+    uint16_t time_sec = sys_time.nb_sec; \
     DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev,				\
                     &imu_nb_err,			\
                     &_twi_blmc_nb_err,			\
@@ -69,7 +70,7 @@
                     &guidance_h_mode,			\
                     &guidance_v_mode,			\
                     &electrical.vsupply,		\
-                    &sys_time.nb_sec			\
+                    &time_sec			\
                     );					\
   }
 #else /* !USE_GPS */
@@ -77,6 +78,7 @@
     uint32_t imu_nb_err = 0;						\
     uint8_t twi_blmc_nb_err = 0;					\
     uint8_t  fix = GPS_FIX_NONE;					\
+    uint16_t time_sec = sys_time.nb_sec;                            \
     DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev,					\
                   &imu_nb_err,				\
                   &twi_blmc_nb_err,				\
@@ -89,7 +91,7 @@
                   &guidance_h_mode,			\
                   &guidance_v_mode,			\
                   &electrical.vsupply,		\
-                  &sys_time.nb_sec    \
+                  &time_sec    \
                   );					\
   }
 #endif /* USE_GPS */
@@ -512,11 +514,11 @@
                                 &b2_hff_state.yP[1][1]);    \
   }
 #ifdef GPS_LAG
-#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {	\
+#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {               \
-    DOWNLINK_SEND_HFF_GPS(_trans, _dev,			\
+    DOWNLINK_SEND_HFF_GPS(_trans, _dev,                     \
-                              &b2_hff_rb_last->lag_counter,		\
+                          &(b2_hff_rb_last->lag_counter),   \
-                              &lag_counter_err,	\
+                          &lag_counter_err,                 \
-                              &save_counter);	\
+                          &save_counter);                   \
   }
 #else
 #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
@@ -747,12 +749,8 @@
                    );                          \
   }
 
-//TODO replace by BOOZ_EXTRA_ADC
+// FIXME: still used?? or replace by EXTRA_ADC
-#ifdef BOOZ2_SONAR
-#define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) DOWNLINK_SEND_BOOZ2_SONAR(_trans, _dev,&booz2_adc_1,&booz2_adc_2,&booz2_adc_3,&booz2_adc_4);
-#else
 #define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) {}
-#endif
 
 #ifdef BOOZ2_TRACK_CAM
 #include "cam_track.h"

sw/airborne/lisa/test/lisa_test_ms2100.c

@@ -70,11 +70,11 @@ static inline void main_periodic_task( void ) {
 
   switch(ms2100_status) {
   case MS2100_IDLE:
-    Ms2001SendReq();
+    Ms2100SendReq();
     break;
   case MS2100_WAITING_EOC:
-    if (Ms2001HasEOC()) {
+    if (Ms2100HasEOC()) {
-      Ms2001ReadRes();
+      Ms2100ReadRes();
     }
     break;
   }

sw/airborne/modules/nav/nav_catapult.c

@@ -64,7 +64,7 @@ static uint16_t nav_catapult_launch = 0;
 #endif
 
 #ifndef NAV_CATAPULT_MOTOR_DELAY
-#define NAV_CATAPULT_MOTOR_DELAY  20		// Main Control Loops
+#define NAV_CATAPULT_MOTOR_DELAY  45		// Main Control Loops
 #endif
 
 #define NAV_CATAPULT_HEADING_DELAY (60 * 3)
@@ -124,7 +124,7 @@ bool_t nav_catapult_init(void)
 
 
 
-bool_t nav_catapult(uint8_t _climb) 
+bool_t nav_catapult(uint8_t _to, uint8_t _climb) 
 {
   float alt = WaypointAlt(_climb);
 
@@ -154,6 +154,10 @@ bool_t nav_catapult(uint8_t _climb)
     NavVerticalThrottleMode(9600*(0));
 
     // Store take-off waypoint
+    WaypointX(_to) = GetPosX();
+    WaypointY(_to) = GetPosY();
+    WaypointAlt(_to) = GetPosAlt();
+
     nav_catapult_x = estimator_x;
     nav_catapult_y = estimator_y;
 
@@ -192,3 +196,14 @@ bool_t nav_catapult(uint8_t _climb)
 return TRUE;
 
 }	// end of gls()
+
+bool_t nav_select_touch_down(uint8_t _td)
+{
+  WaypointX(_td) = GetPosX();
+  WaypointY(_td) = GetPosY();
+  WaypointAlt(_td) = GetPosAlt();
+  return FALSE;
+}
+
+
+

sw/airborne/modules/nav/nav_catapult.h

@@ -39,7 +39,9 @@ void nav_catapult_highrate_module(void);
 extern bool_t nav_catapult_init(void);
 
 extern bool_t nav_catapult_arm(void);
-extern bool_t nav_catapult(uint8_t _climb);
+extern bool_t nav_catapult(uint8_t _to, uint8_t _climb);
 extern bool_t nav_catapult_disarm(void);
 
+extern bool_t nav_select_touch_down(uint8_t _td);
+
 #endif

sw/airborne/subsystems/gps.h

@@ -37,10 +37,6 @@
 #include GPS_TYPE_H
 #endif
 
-#define GPS_FIX_NONE 0x00
-#define GPS_FIX_2D   0x02
-#define GPS_FIX_3D   0x03
-
 #define GpsFixValid() (gps.fix == GPS_FIX_3D)
 
 

sw/airborne/subsystems/gps/gps_mtk.c

@@ -156,16 +156,7 @@ void gps_mtk_read_message(void) {
       gps.speed_3d    = gps.gspeed;
       gps.course      = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
       gps.num_sv      = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
-      switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
+      gps.fix         = MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf);
-      case MTK_DIY_FIX_3D:
-        gps.fix = GPS_FIX_3D;
-        break;
-      case MTK_DIY_FIX_2D:
-        gps.fix = GPS_FIX_2D;
-        break;
-      default:
-        gps.fix = GPS_FIX_NONE;
-      }
       gps.tow         = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
       // FIXME: with MTK DIY 1.4 you do not receive GPS week
       gps.week        = 0;

sw/airborne/subsystems/gps/gps_mtk.h

@@ -28,6 +28,10 @@
 /** Includes macros generated from mtk.xml */
 #include "mtk_protocol.h"
 
+#define GPS_FIX_NONE 0x01
+#define GPS_FIX_2D   0x02
+#define GPS_FIX_3D   0x03
+
 #define GPS_MTK_MAX_PAYLOAD 255
 
 struct GpsMtk {
@@ -105,10 +109,6 @@ extern bool_t gps_configuring;
 extern void gps_mtk_read_message(void);
 extern void gps_mtk_parse(uint8_t c);
 
-#define MTK_DIY_FIX_3D      3
-#define MTK_DIY_FIX_2D      2
-#define MTK_DIY_FIX_NONE    1
-
 /*
  * dynamic GPS configuration
  */

sw/airborne/subsystems/gps/gps_nmea.c

@@ -37,7 +37,7 @@
 
 #include "led.h"
 
-#ifdef GPS_USE_LATLONG
+#if GPS_USE_LATLONG
 #include "subsystems/nav.h"
 #include "math/pprz_geodetic_float.h"
 #endif

sw/airborne/subsystems/gps/gps_nmea.h

@@ -20,8 +20,8 @@
  *
  */
 
-/** \file gps_nmea.h
+/** @file gps_nmea.h
- * \brief NMEA protocol specific code
+ *  NMEA protocol specific code
  *
 */
 
@@ -33,6 +33,10 @@
 
 #define GPS_NB_CHANNELS 16
 
+#define GPS_FIX_NONE 0x00
+#define GPS_FIX_2D   0x02
+#define GPS_FIX_3D   0x03
+
 #ifdef DEBUG_NMEA
 #define NMEA_PRINT(...) {  UsbSPrintString( __VA_ARGS__);};
 #else

sw/airborne/subsystems/gps/gps_sim.h

@@ -3,6 +3,10 @@
 
 #include "std.h"
 
+#define GPS_FIX_NONE 0x00
+#define GPS_FIX_2D   0x02
+#define GPS_FIX_3D   0x03
+
 #define GPS_NB_CHANNELS 16
 
 extern bool_t gps_available;

sw/airborne/subsystems/gps/gps_sim_nps.h

@@ -3,6 +3,10 @@
 
 #include "nps_sensors.h"
 
+#define GPS_FIX_NONE 0x00
+#define GPS_FIX_2D   0x02
+#define GPS_FIX_3D   0x03
+
 #define GPS_NB_CHANNELS 16
 
 extern bool_t gps_available;

sw/airborne/subsystems/gps/gps_skytraq.c

@@ -21,6 +21,11 @@
 
 #include "subsystems/gps.h"
 
+#if GPS_USE_LATLONG
+#include "subsystems/nav.h"
+#include "math/pprz_geodetic_float.h"
+#endif
+
 struct GpsSkytraq gps_skytraq;
 
 /* parser status */
@@ -68,6 +73,23 @@ void gps_skytraq_read_message(void) {
     gps.num_sv      = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf);
     gps.fix         = SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf);
     gps.tow         = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10;
+
+#if GPS_USE_LATLONG
+    /* Computes from (lat, long) in the referenced UTM zone */
+    struct LlaCoor_f lla_f;
+    lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+    lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+    struct UtmCoor_f utm_f;
+    utm_f.zone = nav_utm_zone0;
+    /* convert to utm */
+    utm_of_lla_f(&utm_f, &lla_f);
+    /* copy results of utm conversion */
+    gps.utm_pos.east = utm_f.east*100;
+    gps.utm_pos.north = utm_f.north*100;
+    gps.utm_pos.alt = utm_f.alt*1000;
+    gps.utm_pos.zone = nav_utm_zone0;
+#else
+
     //DEBUG_S2_TOGGLE();
 
 #ifdef GPS_LED

sw/airborne/subsystems/gps/gps_skytraq.h

@@ -24,6 +24,13 @@
 
 #include "mcu_periph/uart.h"
 
+
+#define GPS_FIX_NONE    0x00
+#define GPS_FIX_2D      0x01
+#define GPS_FIX_3D      0x02
+#define GPS_FIX_3D_DGPS 0x03
+
+
 #define SKYTRAQ_SYNC1 0xA0
 #define SKYTRAQ_SYNC2 0xA1
 

sw/airborne/subsystems/gps/gps_ubx.c

@@ -24,7 +24,7 @@
 
 #include "led.h"
 
-#ifdef GPS_USE_LATLONG
+#if GPS_USE_LATLONG
 #include "subsystems/nav.h"
 #include "math/pprz_geodetic_float.h"
 #endif
@@ -124,7 +124,7 @@ void gps_ubx_read_message(void) {
       gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
       gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
       gps.hmsl        = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
-#ifdef GPS_USE_LATLONG
+#if GPS_USE_LATLONG
       /* Computes from (lat, long) in the referenced UTM zone */
       struct LlaCoor_f lla_f;
       lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;

sw/airborne/subsystems/gps/gps_ubx.h

@@ -38,6 +38,10 @@
 
 #define GPS_NB_CHANNELS 16
 
+#define GPS_FIX_NONE 0x00
+#define GPS_FIX_2D   0x02
+#define GPS_FIX_3D   0x03
+
 #define GPS_UBX_MAX_PAYLOAD 255
 struct GpsUbx {
   bool_t msg_available;