Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 15:30:08 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[fixedwing] try to fix nav.c

Browse Source 
this still needs some serious cleanup and refactoring!
This commit is contained in:
Felix Ruess
2016-02-16 22:17:37 +01:00
parent 0aa67a112b
commit 620cb526d6
2 changed files with 34 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/nav.c
+17 -22
View File
@@ -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,15 +155,14 @@ void nav_circle_XY(float x, float y, float radius)
}
#define NavGlide(_last_wp, _wp) { \
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
void nav_glide(uint8_t start_wp, uint8_t wp)
{
float start_alt = waypoints[start_wp].a;
float diff_alt = waypoints[wp].a - start_alt;
float alt = start_alt + nav_leg_progress * diff_alt;
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
NavVerticalAltitudeMode(alt, pre_climb);
}
#define MAX_DIST_CARROT 250.
@@ -223,8 +221,8 @@ static void nav_ground_speed_loop(void)
}
#endif
static 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)
float baseleg_out_qdr;
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
+17 -4
View File
@@ -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);
void nav_home(void);
void nav_init(void);
void nav_without_gps(void);
extern void nav_periodic_task(void);
extern void nav_home(void);
extern void nav_init(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)