diff --git a/conf/airframes/funjet1.xml b/conf/airframes/funjet1.xml
index e9f4339205..f5a1be44c0 100644
--- a/conf/airframes/funjet1.xml
+++ b/conf/airframes/funjet1.xml
@@ -145,15 +145,15 @@
-
+
-
+
-
+
@@ -174,7 +174,7 @@
-
+
diff --git a/conf/airframes/microjet6.xml b/conf/airframes/microjet6.xml
index 4fc19cf414..9d01d4c492 100755
--- a/conf/airframes/microjet6.xml
+++ b/conf/airframes/microjet6.xml
@@ -98,7 +98,7 @@
-
+
@@ -168,6 +168,8 @@
diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml
index 052d2eb499..86279fcb9f 100644
--- a/conf/flight_plans/IS.xml
+++ b/conf/flight_plans/IS.xml
@@ -46,6 +46,11 @@
+
+
+
+
+
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index f631a14fdc..b56c8c8406 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -33,7 +33,7 @@
-
+
@@ -133,5 +133,9 @@
+
+
+
+
diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index c2bbaa5d6b..a5b5a5efe9 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -40,6 +40,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -76,6 +89,7 @@
+
diff --git a/sw/airborne/fw_v_ctl.c b/sw/airborne/fw_v_ctl.c
index 8d9d3e7d8f..6e73d1cf91 100644
--- a/sw/airborne/fw_v_ctl.c
+++ b/sw/airborne/fw_v_ctl.c
@@ -216,6 +216,8 @@ inline static void v_ctl_climb_auto_pitch_loop(void) {
#ifndef V_CTL_THROTTLE_SLEW
#define V_CTL_THROTTLE_SLEW 1.
#endif
+/** \brief Computes slewed throttle from throttle setpoint
+ */
void v_ctl_throttle_slew( void ) {
pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 51455bbe02..379c141686 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -319,8 +319,6 @@ static void navigation_task( void ) {
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
- /** Default to keep compatibility with previous behaviour */
- lateral_mode = LATERAL_MODE_COURSE;
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 80cdf2c1fb..5213cb2f9b 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -79,6 +79,8 @@ float flight_altitude;
float nav_glide_pitch_trim;
+float nav_ground_speed_setpoint, nav_ground_speed_pgain;
+
void nav_init_stage( void ) {
last_x = estimator_x; last_y = estimator_y;
stage_time = 0;
@@ -206,13 +208,9 @@ void nav_circle_XY(float x, float y, float radius) {
#define NavBlockTime() (block_time)
#define LessThan(_x, _y) ((_x) < (_y))
-#define NavFollow(_ac_id, _distance, _height) { \
- struct ac_info_ * ac = get_ac_info(_ac_id); \
- NavVerticalAutoThrottleMode(0.); \
- NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.); \
- float alpha = M_PI/2 - ac->course; \
- fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
-}
+#define NavFollow(_ac_id, _distance, _height) \
+ nav_follow(_ac_id, _distance, _height);
+
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
@@ -317,12 +315,31 @@ static unit_t unit __attribute__ ((unused));
static unit_t nav_reset_reference( void ) __attribute__ ((unused));
static unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused));
+static inline void nav_follow(uint8_t _ac_id, float _distance, float _height);
#include "flight_plan.h"
float ground_alt;
static float previous_ground_alt;
+
+static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
+ struct ac_info_ * ac = get_ac_info(_ac_id);
+ NavVerticalAutoThrottleMode(0.);
+ NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
+ float alpha = M_PI/2 - ac->course;
+ float ca = cos(alpha), sa = sin(alpha);
+ float x = ac->east - _distance*ca;
+ float y = ac->north - _distance*sa;
+ fly_to_xy(x, y);
+#ifdef NAV_FOLLOW_PGAIN
+ float s = (estimator_x-x)*ca+(estimator_y-y)*sa;
+ nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
+ nav_ground_speed_loop();
+#endif
+}
+
+
/** Reset the geographic reference to the current GPS fix */
static unit_t nav_reset_reference( void ) {
@@ -385,16 +402,20 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
}
-/** static inline void fly_to_xy(float x, float y)
+/**
* \brief Computes \a desired_x, \a desired_y and \a desired_course.
*/
static inline void fly_to_xy(float x, float y) {
desired_x = x;
desired_y = y;
h_ctl_course_setpoint = M_PI/2.-atan2(y - estimator_y, x - estimator_x);
+ lateral_mode = LATERAL_MODE_COURSE;
}
+/**
+ * \brief Computes the carrot position along the desired segment.
+ */
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_x = wp_x - last_wp_x;
float leg_y = wp_y - last_wp_y;
@@ -418,8 +439,8 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
}
-/** \brief Compute square distance to waypoint Home and
- * verify uav is not too far away from Home.
+/** \brief Computes square distance to the HOME waypoint potentially sets
+ * \a too_far_from_home
*/
static inline void compute_dist2_to_home(void) {
float ph_x = waypoints[WP_HOME].x - estimator_x;
@@ -436,8 +457,7 @@ static inline void compute_dist2_to_home(void) {
#define FAILSAFE_HOME_RADIUS 50
#endif
-/** \brief Occurs when it switchs in Home mode.
- */
+/** \brief Home mode navigation (circle around HOME) */
void nav_home(void) {
NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS);
/** Nominal speed */
@@ -448,13 +468,17 @@ void nav_home(void) {
dist2_to_wp = dist2_to_home;
}
-/** void nav_update(void)
- * \brief Update navigation
+/**
+ * \brief Navigation main: call to the code generated from the XML flight
+ * plan
*/
void nav_update(void) {
compute_dist2_to_home();
- auto_nav();
+
+ auto_nav(); /* From flight_plan.h */
+
h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
+
#ifdef AGR_CLIMB
if ( v_ctl_mode == V_CTL_MODE_AUTO_CLIMB)
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
@@ -469,40 +493,55 @@ void nav_update(void) {
}
-/** void nav_init(void)
- * \brief Initialisation of navigation
+/**
+ * \brief Navigation Initialisation
*/
void nav_init(void) {
nav_block = 0;
nav_stage = 0;
ground_alt = GROUND_ALT;
nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
+ nav_radius = DEFAULT_CIRCLE_RADIUS;
+
+#ifdef NAV_GROUND_SPEED_PGAIN
+ nav_ground_speed_pgain = NAV_GROUND_SPEED_PGAIN;
+ nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
+#endif
}
-/** void nav_wihtout_gps(void)
- * \brief
- */
-/** Don't navigate anymore. It's impossible without GPS.
- * Just set attitude and throttle to failsafe values
- * to prevent the plane from crashing.
+/**
+ * \brief Failsafe navigation without position estimation
+ *
+ * Just set attitude and throttle to FAILSAFE values
+ * to prevent the plane from crashing.
*/
void nav_without_gps(void) {
-#ifdef SECTION_FAILSAFE
lateral_mode = LATERAL_MODE_ROLL;
+ v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
+
+#ifdef SECTION_FAILSAFE
h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
nav_pitch = FAILSAFE_DEFAULT_PITCH;
- v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE)*MAX_PPRZ);
#else
- lateral_mode = LATERAL_MODE_ROLL;
h_ctl_roll_setpoint = 0;
nav_pitch = 0;
- v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
nav_throttle_setpoint = TRIM_UPPRZ((CRUISE_THROTTLE)*MAX_PPRZ);
#endif
}
+#ifdef NAV_GROUND_SPEED_PGAIN
+/** \brief Computes cruise throttle from ground speed setpoint
+ */
+int nav_ground_speed_loop( void ) {
+ float err = estimator_hspeed_mod - nav_ground_speed_setpoint;
+ v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err;
+ Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+ return FALSE;
+}
+#endif
+
/**************** 8 Navigation **********************************************/
@@ -542,24 +581,28 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
}
/* The other center */
- struct point c2 = { waypoints[target].x - d*u_x,
- waypoints[target].y - d*u_y,
- alt };
+ struct point c2 = {
+ waypoints[target].x - d*u_x,
+ waypoints[target].y - d*u_y,
+ alt };
-
- struct point c1_in = { waypoints[c1].x + radius * -u_y,
- waypoints[c1].y + radius * u_x,
- alt };
- struct point c1_out = { waypoints[c1].x - radius * -u_y,
- waypoints[c1].y - radius * u_x,
- alt };
+ struct point c1_in = {
+ waypoints[c1].x + radius * -u_y,
+ waypoints[c1].y + radius * u_x,
+ alt };
+ struct point c1_out = {
+ waypoints[c1].x - radius * -u_y,
+ waypoints[c1].y - radius * u_x,
+ alt };
- struct point c2_in = { c2.x + radius * -u_y,
- c2.y + radius * u_x,
- alt };
- struct point c2_out = { c2.x - radius * -u_y,
- c2.y - radius * u_x,
- alt };
+ struct point c2_in = {
+ c2.x + radius * -u_y,
+ c2.y + radius * u_x,
+ alt };
+ struct point c2_out = {
+ c2.x - radius * -u_y,
+ c2.y - radius * u_x,
+ alt };
float qdr_out = M_PI - atan2(u_y, u_x);
@@ -586,8 +629,8 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
eight_status = R21;
InitStage();
}
- return;
-
+ return;
+
case R21:
nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index ee3b17c44a..a3d2879b90 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -112,6 +112,8 @@ extern float survey_west, survey_east, survey_north, survey_south;
extern float nav_radius;
+extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
+
void nav_update(void);
void nav_home(void);
@@ -188,6 +190,15 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
nav_throttle_setpoint = _throttle; \
}
+#define NavHeading(_course) { \
+ lateral_mode = LATERAL_MODE_COURSE; \
+ h_ctl_course_setpoint = _course; \
+}
+
+#define NavAttitude(_roll) { \
+ lateral_mode = LATERAL_MODE_ROLL; \
+ h_ctl_roll_setpoint = _roll; \
+}
#endif /* NAV_H */
diff --git a/sw/airborne/traffic_info.c b/sw/airborne/traffic_info.c
index 1054c78d0c..7347eaef56 100644
--- a/sw/airborne/traffic_info.c
+++ b/sw/airborne/traffic_info.c
@@ -31,6 +31,6 @@
struct ac_info_ * get_ac_info(uint8_t id) {
- id = (id < NB_ACS ? id : 0);
+ id = id < NB_ACS ? id : 0;
return &the_acs[id];
}
diff --git a/sw/airborne/traffic_info.h b/sw/airborne/traffic_info.h
index 35f36a5ced..ec9daedc28 100644
--- a/sw/airborne/traffic_info.h
+++ b/sw/airborne/traffic_info.h
@@ -49,7 +49,6 @@ struct ac_info_ the_acs[NB_ACS];
the_acs[_id].course = _course; \
the_acs[_id].alt = _alt; \
the_acs[_id].gspeed = _gspeed; \
- /*** printf("%d:x=%.0f y=%.0f c=%f a=%.0f\n",_id,the_acs[_id].east,the_acs[_id].north,the_acs[_id].course, the_acs[_id].alt); ***/ \
} \
}
diff --git a/sw/logalizer/plot_roll_loop.c b/sw/logalizer/plot_roll_loop.c
index 7ad98d7578..52a346dc7a 100644
--- a/sw/logalizer/plot_roll_loop.c
+++ b/sw/logalizer/plot_roll_loop.c
@@ -35,7 +35,7 @@ GtkWidget* build_gui ( void ) {
void on_DESIRED(IvyClientPtr app, void *user_data, int argc, char *argv[]){
- float p = atof(argv[0]);
+ // float p = atof(argv[0]);
float phi = atof(argv[1]);
float phi_sp = atof(argv[2]);
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index ad484edee5..6f8b049979 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -113,7 +113,7 @@ let get_index_block = fun x ->
let print_exception = fun x ->
let i = get_index_block (ExtXml.attrib x "deroute") in
let c = parsed_attrib x "cond" in
- lprintf "if ((nav_block!=%s) && %s) { GotoBlock(%s) }\n" i c i
+ lprintf "if (%s && (nav_block != %s)) { GotoBlock(%s) }\n" c i i
let goto l = Xml.Element ("goto", ["name",l], [])
let exit_block = Xml.Element ("exit_block", [], [])
@@ -301,7 +301,7 @@ let rec print_stage = fun index_of_waypoints x ->
let until = parsed_attrib x "until" in
lprintf "if (%s) NextStage() else {\n" until;
right ();
- lprintf "h_ctl_course_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "course");
+ lprintf "NavHeading(RadOfDeg(%s));\n" (parsed_attrib x "course");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";
@@ -324,8 +324,7 @@ let rec print_stage = fun index_of_waypoints x ->
lprintf "{\n"
end;
right ();
- lprintf "lateral_mode = LATERAL_MODE_ROLL;\n";
- lprintf "h_ctl_roll_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "roll");
+ lprintf "NavAttitude(RadOfDeg(%s));\n" (parsed_attrib x "roll");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";