[fixedwing] try to fix nav.c

this still needs some serious cleanup and refactoring!

sw/airborne/firmwares/fixedwing/nav.c

@@ -25,16 +25,18 @@
  *
  */
 
-#define NAV_C
-
 #include <math.h>
 
+#define NAV_C
 #include "firmwares/fixedwing/nav.h"
-#include "subsystems/gps.h"
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/autopilot.h"
 #include "inter_mcu.h"
 #include "subsystems/navigation/traffic_info.h"
+#include "subsystems/gps.h"
+
+#include "generated/flight_plan.h"
+
 
 #define RCLost() bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)
 
@@ -43,7 +45,7 @@ enum oval_status oval_status;
 float last_x, last_y;
 
 /** Index of last waypoint. Used only in "go" stage in "route" horiz mode */
-static uint8_t last_wp __attribute__((unused));
+uint8_t last_wp __attribute__((unused));
 
 float rc_pitch;
 float carrot_x, carrot_y;
@@ -100,9 +102,6 @@ void nav_init_stage(void)
   nav_shift = 0;
 }
 
-#define PowerVoltage() (vsupply/10.)
-#define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
-
 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
 
 
@@ -156,17 +155,16 @@ void nav_circle_XY(float x, float y, float radius)
 }
 
 
-#define NavGlide(_last_wp, _wp) {                                       \
+void nav_glide(uint8_t start_wp, uint8_t wp)
-    float start_alt = waypoints[_last_wp].a;                            \
+{
-    float diff_alt = waypoints[_wp].a - start_alt;                      \
+  float start_alt = waypoints[start_wp].a;
-    float alt = start_alt + nav_leg_progress * diff_alt;                \
+  float diff_alt = waypoints[wp].a - start_alt;
-    float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length; \
+  float alt = start_alt + nav_leg_progress * diff_alt;
-    NavVerticalAltitudeMode(alt, pre_climb);                            \
+  float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
+  NavVerticalAltitudeMode(alt, pre_climb);
 }
 
 
-
-
 #define MAX_DIST_CARROT 250.
 #define MIN_HEIGHT_CARROT 50.
 #define MAX_HEIGHT_CARROT 150.
@@ -223,8 +221,8 @@ static void nav_ground_speed_loop(void)
 }
 #endif
 
-static float baseleg_out_qdr;
+float baseleg_out_qdr;
-static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
+bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
 {
   nav_radius = radius;
 
@@ -247,7 +245,7 @@ static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t w
   return FALSE;
 }
 
-static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
+bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
 {
 
   float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
@@ -285,9 +283,6 @@ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float g
 }
 
 
-#include "generated/flight_plan.h"
-
-
 #ifndef LINE_START_FUNCTION
 #define LINE_START_FUNCTION {}
 #endif

sw/airborne/firmwares/fixedwing/nav.h

@@ -45,6 +45,10 @@
 #define Square(_x) ((_x)*(_x))
 #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y))
 
+#define PowerVoltage() (vsupply/10.)
+#define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
+
+
 enum oval_status { OR12, OC2, OR21, OC1 };
 
 extern float cur_pos_x;
@@ -69,6 +73,8 @@ extern bool_t nav_in_segment;
 extern float nav_circle_x, nav_circle_y, nav_circle_radius; /* m */
 extern float nav_segment_x_1, nav_segment_y_1, nav_segment_x_2, nav_segment_y_2; /* m */
 
+extern uint8_t last_wp __attribute__((unused));
+
 extern int nav_mode;
 #define NAV_MODE_ROLL 1
 #define NAV_MODE_COURSE 2
@@ -108,14 +114,21 @@ extern float nav_survey_shift;
 extern float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south;
 extern bool_t nav_survey_active;
 
-void nav_periodic_task(void);
+extern void nav_periodic_task(void);
-void nav_home(void);
+extern void nav_home(void);
-void nav_init(void);
+extern void nav_init(void);
-void nav_without_gps(void);
+extern void nav_without_gps(void);
 
 extern float nav_circle_trigo_qdr; /** Angle from center to mobile */
 extern void nav_circle_XY(float x, float y, float radius);
 
+extern float baseleg_out_qdr;
+extern bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius);
+extern bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide);
+
+extern void nav_glide(uint8_t start_wp, uint8_t wp);
+#define NavGlide(_start_wp, _wp) nav_glide(_start_wp, _wp)
+
 #define NavCircleWaypoint(wp, radius) \
   nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)