[navigaiton] Rotorcraft navigtation parameters belong in the airframe…

… file and should be tunable

backport of #1315

conf/airframes/BR/ladybird_kit_bart.xml

@@ -206,6 +206,11 @@
    <define name="IGAIN" value="24"/>
  </section>
 
+  <section name="NAVIGATION" prefix="NAV_">
+    <define  name="CLIMB_VSPEED" value="1.0" />
+    <define  name="DESCEND_VSPEED" value="-0.5" />
+  </section>
+
  <section name="SIMULATOR" prefix="NPS_">
    <define name="ACTUATOR_NAMES" value="{&quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;, &quot;nw_motor&quot;}"/>
    <define name="JSBSIM_MODEL" value="&quot;simple_x_quad_ccw&quot;"/>

conf/airframes/HooperFly/racerpex_hexa_lisa_mx_20.xml

@@ -213,6 +213,11 @@
     <define name="IGAIN" value="20"/>
   </section>
 
+  <section name="NAVIGATION" prefix="NAV_">
+    <define  name="CLIMB_VSPEED" value="1.0" />
+    <define  name="DESCEND_VSPEED" value="-1.0" />
+  </section>
+
   <section name="SIMULATOR" prefix="NPS_">
     <define name="ACTUATOR_NAMES"  value="{&quot;fl_motor&quot;, &quot;fr_motor&quot;, &quot;r_motor&quot;, &quot;br_motor&quot;, &quot;bl_motor&quot;, &quot;l_motor&quot;}"/>
     <define name="JSBSIM_MODEL"    value="&quot;HooperFly/racerpex_hexa&quot;"/>

conf/airframes/examples/bebop.xml

@@ -196,6 +196,11 @@
     <define name="IGAIN" value="30"/>
   </section>
 
+  <section name="NAVIGATION" prefix="NAV_">
+    <define  name="CLIMB_VSPEED" value="2.5" />
+    <define  name="DESCEND_VSPEED" value="-1.0" />
+  </section>
+
   <section name="SIMULATOR" prefix="NPS_">
     <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
     <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>

conf/flight_plans/rotorcraft_basic.xml

@@ -36,7 +36,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay vmode="climb" climb="0.5" wp="CLIMB"/>
+      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -81,7 +81,7 @@
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
       <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>

conf/flight_plans/rotorcraft_basic_superbitrf.xml

@@ -104,7 +104,7 @@
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_decend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
       <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>

conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml

@@ -82,7 +82,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
      <!-- To make sure that takoff is straight up -->
     <attitude pitch="0" roll="0" throttle="0.90" until="stage_time > 1" vmode="throttle"/>
 <!--Alternative <exception cond="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" deroute="A_to_B_and_back"/>-->
-     <stay vmode="climb" climb="0.9" until="WaypointAlt(WP_STDBY) > stateGetPositionEnu_f()->z" wp="CLIMB"/>
+     <stay vmode="climb" climb="nav_climb_vspeed" until="WaypointAlt(WP_STDBY) > stateGetPositionEnu_f()->z" wp="CLIMB"/>
      </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="if(!InsideKill(GetPosX(), GetPosY())) NavKillThrottle()">
       <exception cond="block_time > 60" deroute="land"/>
@@ -118,7 +118,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
       <go wp="TD"/>
       <exception cond="NavDetectGround()" deroute="landed"/>
       <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
-      <stay climb="-0.5" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="flare">
       <exception cond="NavDetectGround()" deroute="landed"/>

conf/flight_plans/rotorcraft_cam.xml

@@ -35,7 +35,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay climb="0.5" vmode="climb" wp="CLIMB"/>
+      <stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -81,7 +81,7 @@
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
       <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>

conf/flight_plans/rotorcraft_krooz.xml

@@ -46,7 +46,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
 			<!--<call fun="NavStopDetectGround()"/>-->
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
-      <stay vmode="climb" climb="1.0" wp="CLIMB"/>
+      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
     </block>
 		<block name="Come Home">
 			<call fun="NavSetWpCurAlt(WP_STDBY)"/>
@@ -94,7 +94,7 @@
 			<exception cond="RcCommand(RC_MODE_CHANGE) && autopilot_motors_on" deroute="Land here"/>
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
   </blocks>
 </flight_plan>

conf/flight_plans/rotorcraft_optitrack.xml

@@ -50,7 +50,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay climb="0.5" vmode="climb" wp="CLIMB"/>
+      <stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -85,7 +85,7 @@
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
       <call fun="NavKillThrottle()"/>

conf/flight_plans/rotorcraft_survey.xml

@@ -40,7 +40,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay vmode="climb" climb="0.5" wp="CLIMB"/>
+      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -95,7 +95,7 @@
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="TD"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
       <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>

conf/flight_plans/rotorcraft_vision.xml

@@ -27,7 +27,7 @@
     <block name="flare">
       <exception cond="NavDetectGround()" deroute="Holding point"/>
       <call fun="NavStartDetectGround()"/>
-      <stay climb="-0.8" vmode="climb" wp="HOME"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="HOME"/>
     </block>
   </blocks>
 </flight_plan>

conf/settings/control/rotorcraft_guidance.xml

@@ -32,6 +32,8 @@
       <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
       <dl_setting var="nav_heading" MIN="0" STEP="1" MAX="360" module="navigation" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
       <dl_setting var="nav_radius" MIN="-50" STEP="0.1" MAX="50" module="navigation" unit="m"/>
+      <dl_setting var="nav_climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" module="navigation" unit="m/s" param="NAV_CLIMB_VSPEED"/>
+      <dl_setting var="nav_descend_vspeed" MIN="-10.0" STEP="0.1" MAX="0.0" module="navigation" unit="m/s" param="NAV_DESCEND_VSPEED"/>
     </dl_settings>
 
   </dl_settings>

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -81,12 +81,21 @@ bool_t nav_survey_active;
 int32_t nav_roll, nav_pitch;
 int32_t nav_heading;
 float nav_radius;
+float nav_climb_vspeed, nav_descend_vspeed;
 
 /** default nav_circle_radius in meters */
 #ifndef DEFAULT_CIRCLE_RADIUS
 #define DEFAULT_CIRCLE_RADIUS 5.
 #endif
 
+#ifndef NAV_CLIMB_VSPEED
+#define NAV_CLIMB_VSPEED 0.5
+#endif
+
+#ifndef NAV_DESCEND_VSPEED
+#define NAV_DESCEND_VSPEED -0.8
+#endif
+
 uint8_t vertical_mode;
 uint32_t nav_throttle;
 int32_t nav_climb, nav_altitude, nav_flight_altitude;
@@ -160,6 +169,8 @@ void nav_init(void)
   nav_pitch = 0;
   nav_heading = 0;
   nav_radius = DEFAULT_CIRCLE_RADIUS;
+  nav_climb_vspeed = NAV_CLIMB_VSPEED;
+  nav_descend_vspeed = NAV_DESCEND_VSPEED;
   nav_throttle = 0;
   nav_climb = 0;
   nav_leg_progress = 0;

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -55,6 +55,7 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
 extern int32_t nav_roll, nav_pitch;     ///< with #INT32_ANGLE_FRAC
 extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
 extern float nav_radius;
+extern float nav_climb_vspeed, nav_descend_vspeed;
 
 extern int32_t nav_leg_progress;
 extern uint32_t nav_leg_length;