Merge branch 'master' into dev

* nav_catapult
* more fixedwing tuning params
* helicopter safety pilot stuff, stabilization_none

conf/airframes/ENAC/quadrotor/navgo.xml

@@ -37,8 +37,8 @@
   </firmware>
 
   <servos min="0" neutral="0" max="0xff">
-    <servo name="FRONT"  no="2"    min="0" neutral="0" max="255"/>
+    <servo name="FRONT"  no="0"    min="0" neutral="0" max="255"/>
-    <servo name="BACK"   no="0"    min="0" neutral="0" max="255"/>
+    <servo name="BACK"   no="2"    min="0" neutral="0" max="255"/>
     <servo name="RIGHT"  no="1"    min="0" neutral="0" max="255"/>
     <servo name="LEFT"   no="3"    min="0" neutral="0" max="255"/>
   </servos>
@@ -97,53 +97,9 @@
     <define name="MAG_Y_SENS" value="4.56234629213" integer="16"/>
     <define name="MAG_Z_SENS" value="5.298653926" integer="16"/>
 
-    <!--define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="BODY_TO_IMU_PHI"   value="0."/>
-    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0."/>
-    <define name="MAG_Z_NEUTRAL" value="0"/>
+    <define name="BODY_TO_IMU_PSI"   value="0."/>
-
-    <define name="MAG_X_SENS" value="5.43371021972" integer="16"/>
-    <define name="MAG_Y_SENS" value="4.8961742578" integer="16"/>
-    <define name="MAG_Z_SENS" value="5.31527656902" integer="16"/-->
-
-    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/> <!-- -10 -->
-    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/> <!-- -10 -->
-    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
-
-  </section>
-
-  <section name="IMU_PROTO" prefix="IMU_PROTO_">
-
-    <define name="GYRO_P_NEUTRAL" value="-24"/>
-    <define name="GYRO_Q_NEUTRAL" value="-8"/>
-    <define name="GYRO_R_NEUTRAL" value="21"/>
-
-    <!-- SENS ITG3200  1/14.375 (deg/s)/LSB, rate frac 12bit => 1/14.375 * pi / 180 * 2^12 -->
-    <define name="GYRO_P_SENS" value="4.97312" integer="16"/>
-    <define name="GYRO_Q_SENS" value="4.97312" integer="16"/>
-    <define name="GYRO_R_SENS" value="4.97312" integer="16"/>
-
-    <define name="ACCEL_X_NEUTRAL" value="-9"/>
-    <define name="ACCEL_Y_NEUTRAL" value="0"/>
-    <define name="ACCEL_Z_NEUTRAL" value="-29"/>
-
-    <!-- SENS ADXL345 16G 31.2 mg/LSB, accel frac 10bit => 31.2 * 2^10 / 1000 = 31.9488-->
-    <define name="ACCEL_X_SENS" value="38.2683" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="38.7108" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="39.6583" integer="16"/>
-
-
-
-    <define name="MAG_X_NEUTRAL" value="55"/>
-    <define name="MAG_Y_NEUTRAL" value="54"/>
-    <define name="MAG_Z_NEUTRAL" value="92"/>
-
-    <define name="MAG_X_SENS" value="4.97044135787" integer="16"/>
-    <define name="MAG_Y_SENS" value="4.49687342915" integer="16"/>
-    <define name="MAG_Z_SENS" value="4.93138019221" integer="16"/>
-
-    <define name="BODY_TO_IMU_PHI"   value="-10." unit="deg"/>
-    <define name="BODY_TO_IMU_THETA" value="-10." unit="deg"/>
-    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -157,7 +113,8 @@
   </section>
 
   <section name="INS" prefix="INS_">
-    <!-- 1.4 mm/LSB : 0.0014*2^8 = 0.3584 -->
+    <!-- datasheet 1.4 mm/LSB : 0.0014*2^8 = 0.3584 -->
+    <!-- calibration SENS = 1.156 :( -->
     <define name="BARO_SENS" value="1.156" integer="16"/>
   </section>
 
@@ -229,8 +186,8 @@
    <define name="REF_MAX_ZDD"  value=" 0.8*9.81"/>
    <define name="REF_MIN_ZD"   value="-1.5"/>
    <define name="REF_MAX_ZD"   value=" 1.5"/>
-   <define name="HOVER_KP"     value="-500"/>
+   <define name="HOVER_KP"     value="-350"/>
-   <define name="HOVER_KD"     value="-250"/>
+   <define name="HOVER_KD"     value="-160"/>
    <define name="HOVER_KI"     value="0"/>
    <!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
    <define name="RC_CLIMB_COEF" value ="163"/>
@@ -277,11 +234,9 @@
 
 
  <section name="MISC">
-   <define name="BOOZ_ANALOG_BARO_THRESHOLD" value="800"/>
-   <define name="FACE_REINJ_1"  value="1024"/>
    <define name="DEFAULT_CIRCLE_RADIUS" value="10."/>
    <define name="BoozDropPwm(_v)" value="BoozSetPwm1Value(_v)"/>
-   <define name="IMU_MAG_OFFSET" value="RadOfDeg(-9.)"/>
+   <!--define name="IMU_MAG_OFFSET" value="-9."/-->
  </section>
 
   <section name="GCS">

conf/modules/nav_catapult.xml

@@ -0,0 +1,14 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="nav">
+  <header>
+    <file name="nav_catapult.h"/>
+  </header>
+  <init fun="nav_catapult_init()"/>
+  <!-- Run High Rate Lauch Detector -->
+  <periodic fun="nav_catapult_highrate_module()" autorun="TRUE"/>
+  <makefile>
+    <file name="nav_catapult.c"/>
+  </makefile>
+</module>
+

conf/settings/tuning_ins.xml

@@ -58,6 +58,8 @@
 
       <dl_settings name="alt">
         <dl_setting MAX="0" MIN="-0.2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
+        <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>
 
       <dl_settings name="auto_throttle">
@@ -67,6 +69,9 @@
           <strip_button name="Dash" value="1"/>
         </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="0.00" MIN="-0.05" STEP="0.005" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
         <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain" param="V_CTL_AUTO_THROTTLE_IGAIN"/>

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

@@ -42,6 +42,8 @@ float v_ctl_altitude_setpoint;
 float v_ctl_altitude_pre_climb;
 float v_ctl_altitude_pgain;
 float v_ctl_altitude_error;
+float v_ctl_altitude_pre_climb_correction;
+float v_ctl_altitude_max_climb;
 
 /* inner loop */
 float v_ctl_climb_setpoint;
@@ -51,6 +53,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;
@@ -105,6 +109,11 @@ float v_ctl_auto_groundspeed_sum_err;
 #endif
 
 
+#ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
+#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
+#endif
+
+
 void v_ctl_init( void ) {
   /* mode */
   v_ctl_mode = V_CTL_MODE_MANUAL;
@@ -114,6 +123,8 @@ void v_ctl_init( void ) {
   v_ctl_altitude_pre_climb = 0.;
   v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
   v_ctl_altitude_error = 0.;
+  v_ctl_altitude_pre_climb_correction = V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION;
+  v_ctl_altitude_max_climb = V_CTL_ALTITUDE_MAX_CLIMB;
 
   /* inner loops */
   v_ctl_climb_setpoint = 0.;
@@ -122,6 +133,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_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_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle;
   v_ctl_auto_throttle_climb_throttle_increment =
     V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
@@ -195,8 +208,8 @@ void v_ctl_altitude_loop( void ) {
 
   v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
   v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error
-    + v_ctl_altitude_pre_climb;
+    + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction;
-  BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
+  BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb);
 
 #ifdef AGR_CLIMB
   if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
@@ -336,7 +349,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
   controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
 
   // Done, set outputs
-  Bound(controlled_throttle, 0, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+  Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle);
   f_throttle = controlled_throttle;
   nav_pitch = v_ctl_pitch_of_vz;
   v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);

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

@@ -47,6 +47,8 @@ extern float v_ctl_altitude_error;
 extern float v_ctl_altitude_setpoint;
 extern float v_ctl_altitude_pre_climb;
 extern float v_ctl_altitude_pgain;
+extern float v_ctl_altitude_pre_climb_correction;
+extern float v_ctl_altitude_max_climb;
 
 /* inner loop */
 extern float v_ctl_climb_setpoint;
@@ -61,6 +63,8 @@ extern uint8_t v_ctl_auto_throttle_submode;
 
 /* "auto throttle" inner loop parameters */
 extern float v_ctl_auto_throttle_nominal_cruise_throttle;
+extern float v_ctl_auto_throttle_min_cruise_throttle;
+extern float v_ctl_auto_throttle_max_cruise_throttle;
 extern float v_ctl_auto_throttle_cruise_throttle;
 extern float v_ctl_auto_throttle_climb_throttle_increment;
 extern float v_ctl_auto_throttle_pgain;
@@ -110,7 +114,7 @@ extern void v_ctl_throttle_slew( void );
 
 #define guidance_v_SetCruiseThrottle(_v) { \
   v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \
-  Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); \
+  Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \
 }
 
 #define guidance_v_SetAutoThrottleIgain(_v) {	\

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -123,6 +123,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
       autopilot_set_motors_on(FALSE);
       guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
       break;
+    case AP_MODE_RC_DIRECT:
+      guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
+      break;
     case AP_MODE_RATE_DIRECT:
     case AP_MODE_RATE_Z_HOLD:
       guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
@@ -154,6 +157,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
     case AP_MODE_KILL:
       guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
       break;
+    case AP_MODE_RC_DIRECT:
     case AP_MODE_RATE_DIRECT:
     case AP_MODE_ATTITUDE_DIRECT:
     case AP_MODE_HOVER_DIRECT:

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

@@ -109,6 +109,10 @@ void guidance_h_mode_changed(uint8_t new_mode) {
 
   switch (new_mode) {
 
+  case GUIDANCE_H_MODE_RC_DIRECT:
+    stabilization_none_enter();
+    break;
+
   case GUIDANCE_H_MODE_RATE:
     stabilization_rate_enter();
     break;
@@ -137,6 +141,10 @@ void guidance_h_read_rc(bool_t  in_flight) {
 
   switch ( guidance_h_mode ) {
 
+  case GUIDANCE_H_MODE_RC_DIRECT:
+    stabilization_none_read_rc();
+    break;
+
   case GUIDANCE_H_MODE_RATE:
     stabilization_rate_read_rc();
     break;
@@ -168,6 +176,10 @@ void guidance_h_read_rc(bool_t  in_flight) {
 void guidance_h_run(bool_t  in_flight) {
   switch ( guidance_h_mode ) {
 
+  case GUIDANCE_H_MODE_RC_DIRECT:
+    stabilization_none_run(in_flight);
+    break;
+
   case GUIDANCE_H_MODE_RATE:
     stabilization_rate_run(in_flight);
     break;

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

@@ -29,11 +29,12 @@
 
 #include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
 
-#define GUIDANCE_H_MODE_KILL      0
+#define GUIDANCE_H_MODE_KILL        0
-#define GUIDANCE_H_MODE_RATE      1
+#define GUIDANCE_H_MODE_RATE        1
-#define GUIDANCE_H_MODE_ATTITUDE  2
+#define GUIDANCE_H_MODE_ATTITUDE    2
-#define GUIDANCE_H_MODE_HOVER     3
+#define GUIDANCE_H_MODE_HOVER       3
-#define GUIDANCE_H_MODE_NAV       4
+#define GUIDANCE_H_MODE_NAV         4
+#define GUIDANCE_H_MODE_RC_DIRECT   5
 
 
 extern uint8_t guidance_h_mode;

sw/airborne/modules/nav/nav_catapult.c

@@ -0,0 +1,194 @@
+/*
+ *
+ * Copyright (C) 2012, Christophe De Wagter
+ *
+ * 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 module/nav/nav_catapult.h
+ * @brief catapult launch timing system
+ *
+ *
+ * Phase 1: -Zero Roll, Climb Pitch, Zero Throttle
+ * Phase 2: After Feeling the Start Acceleration
+ *          -Zero Roll, Climb Pitch, Full Throttle
+ * Phase 3: After feeling the GPS heading (time based)
+ *          -Place climb 300m in front of us
+ *          -GoTo(climb)
+*/
+
+
+
+#include "generated/airframe.h"
+#include "estimator.h"
+#include "ap_downlink.h"
+#include "modules/nav/nav_catapult.h"
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+#include "firmwares/fixedwing/autopilot.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+
+// Imu is required
+#include "subsystems/imu.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+
+static bool_t nav_catapult_armed = FALSE;
+static uint16_t nav_catapult_launch = 0;
+
+#ifndef NAV_CATAPULT_ACCELERATION_THRESHOLD
+#define NAV_CATAPULT_ACCELERATION_THRESHOLD (1.5 * 9.81);
+#endif
+
+#ifndef NAV_CATAPULT_MOTOR_DELAY
+#define NAV_CATAPULT_MOTOR_DELAY  20		// Main Control Loops
+#endif
+
+#define NAV_CATAPULT_HEADING_DELAY (60 * 3)
+
+static float nav_catapult_x = 0;
+static float nav_catapult_y = 0;
+
+//###############################################################################################
+// Code that Runs in a Fast Module
+
+void nav_catapult_highrate_module(void)
+{
+  // Only run when
+  if (nav_catapult_armed)
+  {
+    if (nav_catapult_launch < NAV_CATAPULT_HEADING_DELAY)
+      nav_catapult_launch ++;
+
+    // Launch detection Filter
+    if (nav_catapult_launch < 5)
+    {
+      // Five consecutive measurements > 1.5
+#ifndef SITL
+      if (ACCEL_FLOAT_OF_BFP(imu.accel.x)  < (1.5f * 9.1))
+#else
+      if (launch != 1)
+#endif
+      {
+        nav_catapult_launch = 0;
+      }
+    }
+    // Launch was detected: Motor Delay Counter
+    else if (nav_catapult_launch == NAV_CATAPULT_MOTOR_DELAY)
+    {
+      // Turn on Motor
+      NavVerticalThrottleMode(9600*(1));
+      launch = 1;
+    }
+  }
+  else
+  {
+    nav_catapult_launch = 0;
+  }
+}
+
+//###############################################################################################
+// Code that runs in 4Hz Nav
+
+bool_t nav_catapult_init(void) 
+{
+
+  nav_catapult_armed = TRUE;
+  nav_catapult_launch = 0;
+
+  return FALSE;
+}
+
+
+
+bool_t nav_catapult(uint8_t _climb) 
+{
+  float alt = WaypointAlt(_climb);
+
+  nav_catapult_armed = 1;
+
+/*
+
+  float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
+  Bound(nav_final_progress,-1,1);
+  float nav_final_length = sqrt(final2);
+
+  float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
+  Bound(pre_climb, -5, 0.);
+
+  float start_alt = WaypointAlt(_tod);
+  float diff_alt = WaypointAlt(_td) - start_alt;
+  float alt = start_alt + nav_final_progress * diff_alt;
+  Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept
+
+*/
+
+  // No Roll, Climb Pitch, No motor Phase
+  if (nav_catapult_launch <= NAV_CATAPULT_MOTOR_DELAY)
+  {
+    NavAttitude(RadOfDeg(0));
+    NavVerticalAutoThrottleMode(RadOfDeg(15));
+    NavVerticalThrottleMode(9600*(0));
+
+    // Store take-off waypoint
+    nav_catapult_x = estimator_x;
+    nav_catapult_y = estimator_y;
+
+  }
+  // No Roll, Climb Pitch, Full Power
+  else if (nav_catapult_launch < NAV_CATAPULT_HEADING_DELAY)
+  {
+    NavAttitude(RadOfDeg(0));
+    NavVerticalAutoThrottleMode(RadOfDeg(15));
+    NavVerticalThrottleMode(9600*(1.0));
+  }
+  // Heading Lock
+  else if (nav_catapult_launch == 0xffff)
+  {
+    NavVerticalAltitudeMode(alt, 0);	// vertical mode (folow glideslope)
+    NavVerticalAutoThrottleMode(RadOfDeg(15));		// throttle mode
+    NavGotoWaypoint(_climb);				// horizontal mode (stay on localiser)
+  }
+  else
+  {
+    // Store Heading, move Climb
+    nav_catapult_launch = 0xffff;
+
+    float dir_x = estimator_x - nav_catapult_x;
+    float dir_y = estimator_y - nav_catapult_y;
+
+    float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
+
+    WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
+    WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
+
+    DownlinkSendWp(DefaultChannel, _climb);
+  }
+
+
+return TRUE;
+
+}	// end of gls()

sw/airborne/modules/nav/nav_catapult.h

@@ -0,0 +1,45 @@
+/*
+ *
+ * Copyright (C) 2012, Christophe De Wagter
+ *
+ * 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 module/nav/nav_catapult.h
+ * @brief catapult launch timing system
+ */
+
+#ifndef NAV_CATAPULT_H
+#define NAV_CATAPULT_H
+
+#include "std.h"
+#include "paparazzi.h"
+
+// Module Code
+void nav_catapult_highrate_module(void);
+
+// Flightplan Code
+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_disarm(void);
+
+#endif

sw/airborne/subsystems/nav.c

@@ -148,11 +148,6 @@ void nav_circle_XY(float x, float y, float radius) {
 }
 
 
-#define NavGotoWaypoint(_wp) { \
-  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
-  fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
-}
-
 #define NavGlide(_last_wp, _wp) { \
   float start_alt = waypoints[_last_wp].a; \
   float diff_alt = waypoints[_wp].a - start_alt; \

sw/airborne/subsystems/nav.h

@@ -76,6 +76,12 @@ extern uint8_t horizontal_mode;
 
 extern void fly_to_xy(float x, float y);
 
+#define NavGotoWaypoint(_wp) { \
+  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
+  fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
+}
+
+
 extern void nav_eight_init( void );
 extern void nav_eight(uint8_t, uint8_t, float);
 #define Eight(a, b, c) nav_eight((a), (b), (c))
@@ -134,6 +140,7 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
 #define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time)
 #define NavApproachingFrom(wp, from, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, time)
 
+
 /** Set the climb control to auto-throttle with the specified pitch
     pre-command */
 #define NavVerticalAutoThrottleMode(_pitch) { \

sw/airborne/subsystems/navigation/OSAMNav.c

@@ -128,6 +128,8 @@ bool_t FlowerNav(void)
 		}
 		break;
 
+	default:
+		break;
 	}
 	return TRUE;
 }
@@ -844,6 +846,8 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
       line_status = LR12;
       nav_init_stage();
     }
+  default:
+    break;
   }
   return TRUE; /* This pattern never ends */
 }

sw/airborne/subsystems/navigation/discsurvey.c

@@ -74,6 +74,8 @@ bool_t disc_survey( uint8_t center, float radius) {
       nav_init_stage();
     }
     break;
+  default:
+    break;
   }
 
   NavVerticalAutoThrottleMode(0.); /* No pitch */

sw/airborne/subsystems/navigation/poly_survey_adv.c

@@ -85,11 +85,11 @@ static void nav_points(point2d start, point2d end)
 **/
 static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
 {
-  float div, fac;
+  float divider, fac;
 
-  div = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1)));
+  divider = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1)));
-  if (div == 0) return FALSE;
+  if (divider == 0) return FALSE;
-  fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / div;
+  fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / divider;
   if (fac > 1.0) return FALSE;
   if (fac < 0.0) return FALSE;
 
@@ -164,7 +164,7 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
 {
   int i;
   point2d small, sweep;
-  float div, len, angle_rad = angle/180.0*M_PI;
+  float divider, len, angle_rad = angle/180.0*M_PI;
 
   if (angle < 0.0) angle += 360.0;
   if (angle >= 360.0) angle -= 360.0;
@@ -224,10 +224,10 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
   small.x = waypoints[poly_first].x;
   small.y = waypoints[poly_first].y;
 
-  div = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
+  divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
 
   //cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
-  if (div < 0.0) {
+  if (divider < 0.0) {
     for(i=1;i<poly_count;i++)
       if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
         small.x = waypoints[poly_first+i].x;

sw/airborne/subsystems/navigation/spiral.c

@@ -162,7 +162,8 @@ bool_t SpiralNav(void)
 	  }
 	  CSpiralStatus = Circle;
 	  break;
-
+       default:
+         break;
 	}
   return TRUE;
 }

sw/ground_segment/cockpit/gcs.ml

@@ -338,6 +338,7 @@ and edit = ref false
 and display_particules = ref false
 and wid = ref None
 and srtm = ref false
+and hide_fp = ref false
 
 let options =
   [
@@ -369,6 +370,7 @@ let options =
    "-utm", Arg.Unit (fun () -> projection:=G.UTM),"Switch to UTM local projection";
    "-wid", Arg.String (fun s -> wid := Some (Int32.of_string s)), "<window id> Id of an existing window to be attached to";
    "-zoom", Arg.Set_float zoom, "Initial zoom";
+   "-auto_hide_fp", Arg.Unit (fun () -> Live.auto_hide_fp true; hide_fp := true), "Automatically hide flight plans of unselected aircraft";
  ]
 
 
@@ -389,6 +391,7 @@ let create_geomap = fun switch_fullscreen editor_frame ->
   ignore (geomap#canvas#event#connect#motion_notify (motion_notify geomap));
   ignore (geomap#canvas#event#connect#any (any_event geomap));
 
+  ignore (menu_fact#add_check_item "Auto hide FP" ~callback:(fun hide -> Live.auto_hide_fp hide) ~active:!hide_fp);
   ignore (menu_fact#add_item "Redraw" ~key:GdkKeysyms._L ~callback:(fun _ -> geomap#canvas#misc#draw None));
   let fullscreen = menu_fact#add_image_item ~stock:(`STOCK "gtk-fullscreen") ~callback:switch_fullscreen () in
   fullscreen#add_accelerator accel_group GdkKeysyms._F11;

sw/ground_segment/cockpit/live.ml

@@ -41,6 +41,8 @@ let gcs_id = "GCS"
 let approaching_alert_time = 3.
 let track_size = ref 500
 
+let _auto_hide_fp = ref false
+
 let min_height = 200
 let lines_height = 30
 
@@ -73,6 +75,7 @@ type aircraft = {
     track : MapTrack.track;
     color: color;
     fp_group : MapFP.flight_plan;
+    fp_show : GMenu.check_menu_item;
     wp_HOME : MapWaypoints.waypoint option;
     fp : Xml.xml;
     blocks : (int * string) list;
@@ -117,6 +120,32 @@ let get_ac = fun vs ->
   let ac_id = Pprz.string_assoc "ac_id" vs in
   find_ac ac_id
 
+let show_fp = fun ac ->
+  ac.fp_group#show ();
+  ac.fp_show#set_active true
+
+let hide_fp = fun ac ->
+  ac.fp_group#hide ();
+  ac.fp_show#set_active false
+  
+(* callback for FP check button in menu *)
+let show_mission = fun ac on_off ->
+  let a = find_ac ac in
+  if on_off then
+    a.fp_group#show ()
+  else
+    a.fp_group#hide ()
+
+let auto_hide_fp = fun hide ->
+  let _hide_fp = fun () ->
+    Hashtbl.iter (fun _ a -> hide_fp a) aircrafts;
+    if !active_ac <> "" then begin
+      let a = find_ac !active_ac in
+      show_fp a
+    end;
+  in
+  _auto_hide_fp := hide;
+  if hide then _hide_fp () else Hashtbl.iter (fun _ a -> show_fp a) aircrafts
 
 let select_ac = fun acs_notebook ac_id ->
   if !active_ac <> ac_id then
@@ -127,11 +156,13 @@ let select_ac = fun acs_notebook ac_id ->
     if !active_ac <> "" then begin
       let ac' = find_ac !active_ac in
       ac'.strip#hide_buttons ();
-      ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text)
+      ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text);
+      if !_auto_hide_fp then hide_fp ac'
     end;
 
     (* Set the new active *)
     active_ac := ac_id;
+    if !_auto_hide_fp then show_fp ac;
 
     (* Select and enlarge the label of the A/C notebook *)
     let n = acs_notebook#page_num ac.pages in
@@ -150,13 +181,6 @@ let log =
 
 let log_and_say = fun a ac_id s -> log ~say:true a ac_id s
 
-let show_mission = fun ac on_off ->
-  let a = find_ac ac in
-  if on_off then
-    a.fp_group#show ()
-  else
-    a.fp_group#hide ()
-
 let resize_track = fun ac track ->
   match
     GToolbox.input_string ~text:(string_of_int track#size) ~title:ac "Track size"
@@ -392,8 +416,8 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
   let ac_menu = GMenu.menu () in
   ac_mi#set_submenu ac_menu;
   let ac_menu_fact = new GMenu.factory ac_menu in
-  let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in
+  let fp_show = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in
-  ignore (fp#connect#toggled (fun () -> show_mission ac_id fp#active));
+  ignore (fp_show#connect#toggled (fun () -> show_mission ac_id fp_show#active));
 
   let track = new MapTrack.track ~size: !track_size ~name ~color:color geomap in
   geomap#register_to_fit (track:>MapCanvas.geographic);
@@ -606,16 +630,16 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
     loop fp#waypoints in
 
   let ac = { track = track; color = color; last_dist_to_wp = 0.;
-         fp_group = fp ; config = config ; wp_HOME = wp_HOME;
+         fp_group = fp; fp_show = fp_show ; config = config ;
-         fp = fp_xml; ac_name = name;
+         wp_HOME = wp_HOME; fp = fp_xml; ac_name = name;
          blocks = blocks; last_ap_mode= "";
          last_stage = (-1,-1);
          ir_page = ir_page; flight_time = 0;
          gps_page = gps_page;
-           pfd_page = pfd_page;
+         pfd_page = pfd_page;
          misc_page = misc_page;
          dl_settings_page = dl_settings_page;
-           rc_settings_page = rc_settings_page;
+         rc_settings_page = rc_settings_page;
          strip = strip; first_pos = true;
          last_block_name = ""; alt = 0.; target_alt = 0.;
          in_kill_mode = false; speed = 0.;
@@ -623,7 +647,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
          wind_speed = 0.;
          pages = ac_frame#coerce;
          notebook_label = _label;
-           got_track_status_timer = 1000;
+         got_track_status_timer = 1000;
          dl_values = [||]; last_unix_time = 0.;
          airspeed = 0.
        } in

sw/ground_segment/cockpit/live.mli

@@ -32,6 +32,7 @@ type aircraft = private {
     track : MapTrack.track;
     color: color;
     fp_group : MapFP.flight_plan;
+    fp_show : GMenu.check_menu_item;
     wp_HOME : MapWaypoints.waypoint option;
     fp : Xml.xml;
     blocks : (int * string) list;
@@ -70,6 +71,9 @@ val safe_bind : string -> (string -> Pprz.values -> unit) -> unit
 val track_size : int ref
 (** Default length for A/C tracks on the 2D view *)
 
+val auto_hide_fp : bool -> unit
+(** Automatically hide flight plan of not selected ac *)
+
 val listen_acs_and_msgs : MapCanvas.widget -> GPack.notebook -> Pages.alert -> bool -> Gtk_tools.pixmap_in_drawin_area -> unit
 (** [listen_acs_and_msgs geomap aircraft_notebook alert_page auto_center_new_ac alt_graph] *)