[nav] small improvements to rotorcaft nav (#3108)

- macro for oval count
- set in_flight to true during takeoff with module
- specify the height of hybrid survey in a parameter

sw/airborne/modules/nav/nav_rotorcraft_base.c

@@ -26,6 +26,7 @@
 
 #include "modules/nav/nav_rotorcraft_base.h"
 #include "firmwares/rotorcraft/navigation.h"
+#include "generated/flight_plan.h"
 
 struct NavBase_t nav_rotorcraft_base;
 
@@ -35,7 +36,6 @@ static void nav_stage_init(void)
 {
   nav_rotorcraft_base.circle.radians = 0.f;
   nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
-  nav_rotorcraft_base.oval.count = 0;
 }
 
 static void nav_goto(struct EnuCoor_f *wp)

sw/airborne/modules/nav/nav_rotorcraft_base.h

@@ -36,10 +36,11 @@ extern struct NavBase_t nav_rotorcraft_base;
 extern void nav_rotorcraft_init(void);
 
 
-/** Macros for circle nav
+/** Macros for circle and oval nav
  */
 #define NavCircleCount() nav_circle_get_count(&nav_rotorcraft_base.circle)
 #define NavCircleQdr() nav_circle_qdr(&nav_rotorcraft_base.circle)
+#define NavOvalCount nav_rotorcraft_base.oval.count
 
 /** True if x (in degrees) is close to the current QDR (less than 10 degrees)
  */

sw/airborne/modules/nav/nav_survey_hybrid.c

@@ -98,7 +98,7 @@ struct SurveyHybridPrivate {
 struct SurveyHybrid survey_hybrid;
 static struct SurveyHybridPrivate survey_private;
 
-static void nav_survey_hybrid_setup(float orientation, float sweep, float radius);
+static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height);
 
 static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans);
 static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans);
@@ -134,7 +134,7 @@ static bool nav_survey_hybrid_mission_local(uint8_t nb, float *params, enum Miss
         survey_private.corners[i].y = params[5+2*i+1];
         survey_private.corners[i].z = height;
       }
-      nav_survey_hybrid_setup(orientation, sweep, radius);
+      nav_survey_hybrid_setup(orientation, sweep, radius, height);
       return nav_survey_hybrid_run();
     }
   }
@@ -164,7 +164,7 @@ static bool nav_survey_hybrid_mission_global(uint8_t nb, float *params, enum Mis
         enu_of_lla_point_f(&corner, &state.ned_origin_f, &lla);
         survey_private.corners[i] = corner;
       }
-      nav_survey_hybrid_setup(orientation, sweep, radius);
+      nav_survey_hybrid_setup(orientation, sweep, radius, height);
       return nav_survey_hybrid_run();
     }
   }
@@ -194,7 +194,7 @@ void nav_survey_hybrid_init(void)
 
 /** finish preparation of survey based on private structure
  */
-static void nav_survey_hybrid_setup(float orientation, float sweep, float radius)
+static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height)
 {
   FLOAT_VECT2_ZERO(survey_private.smallest_corner);
   int i = 0;
@@ -347,7 +347,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius
   // Find the entry point
   survey_private.entry.x = survey_private.from_wp.x;
   survey_private.entry.y = survey_private.corners[0].y + entry_distance;
-  survey_private.entry.z = survey_private.corners[0].z;
+  survey_private.entry.z = height;
 
   // Go into entry state
   survey_private.status = Entry;
@@ -357,7 +357,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius
   survey_private.valid = true;
 }
 
-void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius)
+void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height)
 {
   survey_private.valid = false;
   if (size < 3 || size > SURVEY_HYBRID_MAX_POLYGON_SIZE) {
@@ -372,10 +372,10 @@ void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, ui
   }
   survey_private.size = size;
 
-  nav_survey_hybrid_setup(orientation, sweep, radius);
+  nav_survey_hybrid_setup(orientation, sweep, radius, height);
 }
 
-void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius)
+void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height)
 {
   survey_private.valid = false;
   struct EnuCoor_f *start = waypoint_get_enu_f(start_wp);
@@ -387,7 +387,7 @@ void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_
   float dx = second->x - start->x;
   float dy = second->y - start->y;
   float angle = DegOfRad(atan2f(dy, dx));
-  nav_survey_hybrid_setup_orientation(start_wp, angle, size, sweep, radius);
+  nav_survey_hybrid_setup_orientation(start_wp, angle, size, sweep, radius, height);
 }
 
 //=========================================================================================================================

sw/airborne/modules/nav/nav_survey_hybrid.h

@@ -50,8 +50,9 @@ extern void nav_survey_hybrid_init(void);
  * @param size         number of waypoints/corners used to define the polygon
  * @param sweep        distance between scan lines
  * @param radius       turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
+ * @param height       starting height in meters
  */
-extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius);
+extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height);
 
 /**
  * Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
@@ -60,9 +61,10 @@ extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientat
  * @param second_wp second waypoint towards which the sweep orientation is computed
  * @param size      number of waypoints/corners used to define the polygon
  * @param sweep     distance between scan lines, if zero uses Poly_Distance
- * @param radius       turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
+ * @param radius    turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
+ * @param height    starting height in meters
  */
-extern void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius);
+extern void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height);
 
 /** Run polygon hybrid survey */
 extern bool nav_survey_hybrid_run(void);

sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c

@@ -154,6 +154,7 @@ static bool nav_takeoff_run(void) {
       break;
     case NAV_TAKEOFF_CLIMB:
       // call vertical climb from nav/guidance
+      autopilot_set_in_flight(true);
       NavGotoWaypoint(takeoff.climb_id);
       NavVerticalClimbMode(NAV_TAKEOFF_CLIMB_SPEED);
       if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > NAV_TAKEOFF_HEIGHT) {