[heli][flight_plans] Heli Spinup routine and flight plan mode manual (#1606)

conf/flight_plans/TUDELFT/tudelft_delft_outback.xml

@@ -0,0 +1,98 @@
+<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
+
+<flight_plan alt="4" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
+  <header>
+#include "autopilot.h"
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="0.0"/>
+    <waypoint name="CLIMB" x="0.0" y="5.0"/>
+    <waypoint name="STDBY" x="-2.0" y="-5.0"/>
+    <waypoint name="p1" x="-19.9" y="14.3"/>
+    <waypoint name="p2" x="-0.6" y="21.6"/>
+    <waypoint name="p3" x="22.2" y="-26.5"/>
+    <waypoint name="p4" x="4.9" y="-34.4"/>
+    <waypoint name="CAM" x="14.2" y="-29.4"/>
+    <waypoint name="TD" x="5.6" y="-10.9"/>
+  </waypoints>
+  <modules>
+    <!-- extra navigation routines -->
+    <module name="nav" type="heli_spinup"/>
+  </modules>
+  <blocks>
+    <!--block name="Wait GPS">
+      <call fun="NavKillThrottle()"/>
+      <while cond="!GpsFixValid()"/>
+    </block-->
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+    </block>
+    <block name="Holding point">
+      <call fun="NavKillThrottle()"/>
+      <manual pitch="0" roll="0" yaw="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+    <block name="Start Engine">
+      <call fun="nav_throttle_curve_set(0)" loop="false"/>
+      <call fun="nav_heli_spinup_setup(10, 0.3)" loop="false"/>
+      <call fun="NavResurrect()"/>
+      <call fun="nav_heli_spinup_run()"/>
+    </block>
+    <block name="Hold Attitude">
+      <manual pitch="0" roll="0" yaw="0" throttle="0.3" until="FALSE" vmode="throttle"/>
+    </block>
+    <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="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+      <stay wp="STDBY"/>
+    </block>
+    <block name="stay_p1">
+      <stay wp="p1"/>
+    </block>
+    <block name="go_p2">
+      <call fun="nav_set_heading_deg(90)"/>
+      <go wp="p2"/>
+      <deroute block="stay_p1"/>
+    </block>
+    <block name="line_p1_p2">
+      <go from="p1" hmode="route" wp="p2"/>
+      <stay until="stage_time>10" wp="p2"/>
+      <go from="p2" hmode="route" wp="p1"/>
+      <deroute block="stay_p1"/>
+    </block>
+    <block name="route">
+      <go from="p1" hmode="route" wp="p3"/>
+      <go from="p3" hmode="route" wp="p4"/>
+      <go from="p4" hmode="route" wp="p1"/>
+      <deroute block="stay_p1"/>
+    </block>
+    <block name="test yaw">
+      <go wp="p1"/>
+      <for from="1" to="16" var="i">
+        <heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time > 3"/>
+      </for>
+      <deroute block="Standby"/>
+    </block>
+    <block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
+      <circle radius="nav_radius" wp="CAM"/>
+    </block>
+    <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
+      <call fun="NavSetWaypointHere(WP_TD)"/>
+    </block>
+    <block name="land">
+      <go wp="TD"/>
+    </block>
+    <block name="flare">
+      <exception cond="NavDetectGround()" deroute="Holding point"/>
+      <exception cond="!nav_is_in_flight()" deroute="landed"/>
+      <call fun="NavStartDetectGround()"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
+    </block>
+    <block name="landed">
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>

conf/flight_plans/flight_plan.dtd

@@ -27,17 +27,18 @@
 <!ELEMENT exceptions (exception*)>
 
 <!ELEMENT blocks (block+)>
-<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path)*>
+<!ELEMENT block (exception|while|heading|attitude|manual|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path)*>
 
 <!ELEMENT include (arg|with)*>
 <!ELEMENT arg EMPTY>
 <!ELEMENT with EMPTY>
 
-<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
-<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
+<!ELEMENT while (exception|while|heading|attitude|manual|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
+<!ELEMENT for (exception|while|heading|attitude|manual|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path)*>
 <!ELEMENT exception EMPTY>
 <!ELEMENT heading EMPTY>
 <!ELEMENT attitude EMPTY>
+<!ELEMENT manual EMPTY>
 <!ELEMENT go EMPTY>
 <!ELEMENT xyz EMPTY>
 <!ELEMENT set EMPTY>
@@ -125,7 +126,7 @@ value CDATA #REQUIRED>
 
 <!ATTLIST blocks>
 
-<!ATTLIST block 
+<!ATTLIST block
 name CDATA #REQUIRED
 pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
@@ -146,7 +147,7 @@ var CDATA #REQUIRED
 from CDATA #REQUIRED
 to CDATA #REQUIRED>
 
-<!ATTLIST heading 
+<!ATTLIST heading
 course CDATA #REQUIRED
 vmode CDATA #IMPLIED
 alt CDATA #IMPLIED
@@ -166,7 +167,18 @@ climb CDATA #IMPLIED
 pitch CDATA #IMPLIED
 until CDATA #IMPLIED>
 
-<!ATTLIST go 
+<!ATTLIST manual
+pitch CDATA #REQUIRED
+roll CDATA #REQUIRED
+yaw CDATA #REQUIRED
+vmode CDATA #IMPLIED
+alt CDATA #IMPLIED
+height CDATA #IMPLIED
+throttle CDATA #IMPLIED
+climb CDATA #IMPLIED
+until CDATA #IMPLIED>
+
+<!ATTLIST go
 wp CDATA #REQUIRED
 wp_qdr CDATA #IMPLIED
 wp_dist CDATA #IMPLIED

conf/modules/nav_heli_spinup.xml

@@ -0,0 +1,19 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="nav_heli_spinup" dir="nav">
+  <doc>
+    <description>Helicopter Rotor spinup
+      This module controls the spinup of the main rotor from a Helicopter.
+      - First make sure you are in the correct throttle curve
+      - Next call nav_heli_spinup_setup with a duration and end throttle level
+      - Then call nav_heli_spinup_run
+    </description>
+  </doc>
+  <header>
+    <file name="nav_heli_spinup.h"/>
+  </header>
+  <makefile>
+    <file name="nav_heli_spinup.c"/>
+  </makefile>
+</module>
+

sw/airborne/firmwares/fixedwing/nav.h

@@ -210,6 +210,9 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
     {h_ctl_roll_setpoint = _roll;} \
   }
 
+#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
+
+
 #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; }
 
 #define nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }

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

@@ -404,12 +404,18 @@ void guidance_h_run(bool  in_flight)
         guidance_h_nav_enter();
       }
 
-      if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
+      if(horizontal_mode == HORIZONTAL_MODE_MANUAL) {
+        stabilization_cmd[COMMAND_ROLL]  = nav_roll;
+        stabilization_cmd[COMMAND_PITCH] = nav_pitch;
+        stabilization_cmd[COMMAND_YAW]   = nav_heading;
+      }
+      else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
         struct Int32Eulers sp_cmd_i;
         sp_cmd_i.phi = nav_roll;
         sp_cmd_i.theta = nav_pitch;
         sp_cmd_i.psi = nav_heading;
         stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
+        stabilization_attitude_run(in_flight);
       } else {
         INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
 
@@ -427,8 +433,8 @@ void guidance_h_run(bool  in_flight)
         stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
                                                guidance_h.sp.heading);
 #endif
+        stabilization_attitude_run(in_flight);
       }
-      stabilization_attitude_run(in_flight);
       break;
 
 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -479,6 +479,20 @@ void nav_home(void)
   nav_run();
 }
 
+/** Set manual roll, pitch and yaw without stabilization
+ *
+ * @param[in] roll The angle in radians (float)
+ * @param[in] pitch The angle in radians (float)
+ * @param[in] yaw The angle in radians (float)
+ */
+void nav_set_manual(float roll, float pitch, float yaw)
+{
+  horizontal_mode = HORIZONTAL_MODE_MANUAL;
+  nav_roll = ANGLE_BFP_OF_REAL(roll);
+  nav_pitch = ANGLE_BFP_OF_REAL(pitch);
+  nav_heading = ANGLE_BFP_OF_REAL(yaw);
+}
+
 /** Returns squared horizontal distance to given point */
 float get_dist2_to_point(struct EnuCoor_i *p)
 {

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -52,6 +52,7 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
 #define HORIZONTAL_MODE_ROUTE     1
 #define HORIZONTAL_MODE_CIRCLE    2
 #define HORIZONTAL_MODE_ATTITUDE  3
+#define HORIZONTAL_MODE_MANUAL    4
 extern int32_t nav_roll, nav_pitch;     ///< with #INT32_ANGLE_FRAC
 extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
 extern float nav_radius;
@@ -80,6 +81,7 @@ extern float get_dist2_to_waypoint(uint8_t wp_id);
 extern float get_dist2_to_point(struct EnuCoor_i *p);
 extern void compute_dist2_to_home(void);
 extern void nav_home(void);
+extern void nav_set_manual(float roll, float pitch, float yaw);
 
 unit_t nav_reset_reference(void) __attribute__((unused));
 unit_t nav_reset_alt(void) __attribute__((unused));
@@ -208,6 +210,8 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
     nav_roll = ANGLE_BFP_OF_REAL(_roll); \
   }
 
+#define NavSetManual nav_set_manual
+
 #define NavStartDetectGround() ({ autopilot_detect_ground_once = true; false; })
 #define NavDetectGround() nav_detect_ground()
 

sw/airborne/modules/helicopter/throttle_curve.c

@@ -25,6 +25,7 @@
  */
 
 #include "throttle_curve.h"
+#include "subsystems/commands.h"
 
 /* The switching values for the Throttle Curve Mode switch */
 #define THROTTLE_CURVE_SWITCH_VAL (MAX_PPRZ*2/THROTTLE_CURVES_NB)
@@ -80,3 +81,11 @@ void throttle_curve_run(bool motors_on, pprz_t in_cmd[])
     throttle_curve.throttle = 0;
   }
 }
+
+/**
+ * Set a specific throttle curve based on the mode given with this function
+ */
+void nav_throttle_curve_set(uint8_t mode)
+{
+  commands[COMMAND_FMODE] = mode * THROTTLE_CURVE_SWITCH_VAL - MAX_PPRZ;
+}

sw/airborne/modules/helicopter/throttle_curve.h

@@ -52,5 +52,6 @@ extern struct throttle_curve_t throttle_curve;
 /* External functions */
 extern void throttle_curve_init(void);
 void throttle_curve_run(bool motors_on, pprz_t in_cmd[]);
+void nav_throttle_curve_set(uint8_t mode);
 
 #endif

sw/airborne/modules/nav/nav_heli_spinup.c

@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/nav/nav_heli_spinup.c"
+ * @author Freek van Tienen
+ * This module controls the spinup of the main rotor from a Helicopter
+ */
+
+#include "modules/nav/nav_heli_spinup.h"
+#include "navigation.h"
+#include "paparazzi.h"
+
+static struct nav_heli_spinup_t nav_heli_spinup;
+
+void nav_heli_spinup_setup(uint16_t duration, float throttle)
+{
+  nav_heli_spinup.duration = (duration > 0) ? duration : 1;
+  nav_heli_spinup.throttle = throttle * MAX_PPRZ;
+
+  nav_throttle = 0;
+  nav_roll = 0;
+  nav_pitch = 0;
+  nav_heading = 0;
+  horizontal_mode = HORIZONTAL_MODE_MANUAL;
+  vertical_mode = VERTICAL_MODE_MANUAL;
+}
+
+bool nav_heli_spinup_run(void)
+{
+  if (stage_time > nav_heli_spinup.duration) {
+    return false;
+  }
+
+  nav_roll = 0;
+  nav_pitch = 0;
+  nav_heading = 0;
+  horizontal_mode = HORIZONTAL_MODE_MANUAL;
+  vertical_mode = VERTICAL_MODE_MANUAL;
+  nav_throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
+  return true;
+}

sw/airborne/modules/nav/nav_heli_spinup.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/nav/nav_heli_spinup.h"
+ * @author Freek van Tienen
+ * This module controls the spinup of the main rotor from a Helicopter
+ */
+
+#ifndef NAV_HELI_SPINUP_H
+#define NAV_HELI_SPINUP_H
+
+#include "std.h"
+
+struct nav_heli_spinup_t {
+  uint16_t duration;        ///< The duration in seconds to reach the final throttle
+  uint32_t throttle;        ///< The final throttle level
+};
+
+/** Initialization function
+ *
+ * Called in the flight plan before the 'run' function
+ *
+ * @param[in] duration The duration in seconds before reaching final throttle level
+ * @param[in] throttle The throttle value to go to
+ */
+extern void nav_heli_spinup_setup(uint16_t duration, float throttle);
+
+/** Heli spinup run function
+ *
+ * Controls the spinup of the throttle without stabilization
+ *
+ * @return true until the takeoff procedure ends
+ */
+extern bool nav_heli_spinup_run(void);
+
+#endif

sw/ground_segment/tmtc/server_globals.ml

@@ -14,7 +14,7 @@ let gps_modes = [|"NOFIX";"NA";"2D";"3D";"DGPS";"RTK"|]
 let state_filter_modes = [|"UNKNOWN";"INIT";"ALIGN";"OK";"GPS_LOST";"IMU_LOST";"COV_ERR";"IR_CONTRAST";"ERROR"|]
 let _3D = 3
 let gps_hybrid_modes = [|"OFF";"ON"|]
-let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]
+let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE";"MANUAL"|]
 let if_modes = [|"OFF";"DOWN";"UP"|]
 
 let string_of_values = fun values ->

sw/tools/generators/gen_flight_plan.ml

@@ -213,12 +213,16 @@ let pprz_throttle = fun s ->
 (********************* Vertical control ********************************************)
 let output_vmode = fun stage_xml wp last_wp ->
   let pitch = try Xml.attrib stage_xml "pitch" with _ -> "0.0" in
-  if pitch = "auto"
+  if String.lowercase (Xml.tag stage_xml) <> "manual"
   then begin
-    lprintf "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle"))
-  end else begin
-    lprintf "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch);
+    if pitch = "auto"
+    then begin
+      lprintf "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle"))
+    end else begin
+      lprintf "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch);
+    end
   end;
+
   let vmode = try ExtXml.attrib stage_xml "vmode" with _ -> "alt" in
   begin
     match vmode with
@@ -300,7 +304,7 @@ let rec index_stage = fun x ->
         incr stage; (* To count the loop stage *)
         Xml.Element (Xml.tag x, Xml.attribs x@["no", soi n], l)
       | "return" | "goto"  | "deroute" | "exit_block" | "follow" | "call" | "home"
-      | "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
+      | "heading" | "attitude" | "manual" | "go" | "stay" | "xyz" | "set" | "circle" ->
         incr stage;
         Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
       | "survey_rectangle" | "eight" | "oval"->
@@ -396,6 +400,20 @@ let rec print_stage = fun index_of_waypoints x ->
         ignore (output_vmode x "" "");
         left (); lprintf "}\n";
         lprintf "break;\n"
+      | "manual" ->
+        stage ();
+        begin
+          try
+            let until = parsed_attrib x "until" in
+            lprintf "if (%s) NextStageAndBreak() else {\n" until;
+          with ExtXml.Error _ ->
+            lprintf "{\n"
+        end;
+        right ();
+        lprintf "NavSetManual(%s, %s, %s);\n" (parsed_attrib x "roll") (parsed_attrib x "pitch") (parsed_attrib x "yaw");
+        ignore (output_vmode x "" "");
+        left (); lprintf "}\n";
+        lprintf "break;\n"
       | "go" ->
         stage ();
         let wp =