*** empty log message ***

conf/airframes/funjet1.xml

@@ -145,15 +145,15 @@
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
 
-    <define name="ROLL_PGAIN" value="13000."/>
+    <define name="ROLL_PGAIN" value="10000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
     <define name="PITCH_PGAIN" value="-14000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
-    <define name="ELEVATOR_OF_ROLL" value="1250"/>
+    <define name="ELEVATOR_OF_ROLL" value="2500"/>
     
     <!--  roll rate loop -->
     <define name="ROLL_RATE_MODE_DEFAULT" value="1"/>
-    <define name="ROLL_RATE_SETPOINT_PGAIN" value="-14." unit="rad/s/rad"/>
+    <define name="ROLL_RATE_SETPOINT_PGAIN" value="-10." unit="rad/s/rad"/>
     <define name="ROLL_RATE_MAX_SETPOINT" value="10"/>
     <define name="LO_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
     <define name="HI_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
@@ -174,7 +174,7 @@
     <define name="CLIMB_THROTTLE" value="0.8"/><!-- Gaz for Aggressive Climb -->
     <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
     <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
-    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.15"/><!-- Pitch for Aggressive Decent -->
     <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
     <define name="DESCENT_NAV_RATIO" value="1.0"/>
     </section>

conf/airframes/microjet6.xml

@@ -98,7 +98,7 @@
   </section>
  
   <section name="MISC">
-    <define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
@@ -168,6 +168,8 @@
   <section name="NAV">
     <define name="NAV_PITCH" value="0."/>
     <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+    <define name="NAV_GROUND_SPEED_PGAIN" value="-0.05"/>
+    <define name="NAV_FOLLOW_PGAIN" value="-0.05"/>
   </section>
 
   <section name="AGGRESSIVE" prefix="AGR_">

conf/flight_plans/IS.xml

@@ -46,6 +46,11 @@
       <circle radius="nav_radius" wp="1"/>
     </block>
 
+    <block name="ground speed">
+<!--      <exception cond="nav_ground_speed_loop()" deroute="ground speed"/> -->
+      <circle radius="nav_radius" wp="1"/>
+    </block>
+
     <block name="eight 1" strip_button="8">
       <eight radius="nav_radius" center="1" turn_around="2"/>
     </block>

conf/flight_plans/versatile.xml

@@ -33,7 +33,7 @@
     </block>
 
     <block name="circle 1">
-      <circle radius="75" wp="1"/>
+      <circle radius="nav_radius" wp="1"/>
     </block>
 
     <block name="eight 1">
@@ -133,5 +133,9 @@
       </while>
     </block>
 
+    <block name="heading">
+       <heading until="FALSE" course="30" alt="ground_alt+50"/>
+    </block>
+
   </blocks>
 </flight_plan>

conf/settings/tuning.xml

@@ -40,6 +40,19 @@
         <dl_setting MAX="1" MIN="0" STEP="1" VAR="ir_360" shortname="ir_360"/>
       </dl_settings>
 
+
+      <dl_settings name="rate">
+        <dl_setting MAX="1" MIN="0" STEP="0.1" VAR="h_ctl_roll_rate_mode" shortname="mode"/>
+        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_hi_throttle_roll_rate_pgain" shortname="hi th pgain"/>
+        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_lo_throttle_roll_rate_pgain" shortname="lo th pgain"/>
+        <dl_setting MAX="2" MIN="-2" STEP="0.1" VAR="h_ctl_roll_rate_dgain" shortname="dgain"/>
+        <dl_setting MAX="0" MIN="-20" STEP="1" VAR="h_ctl_roll_rate_setpoint_pgain" shortname="roll_pgain"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" VAR="h_ctl_auto1_rate" shortname="AUTO1 rate"/>
+      </dl_settings>
+
+
+
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain"/>
         <dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain"/>
@@ -76,6 +89,7 @@
         <dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain"/>
         <dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
         <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim"/>
+        <dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_radius"/>
       </dl_settings>
     </dl_settings>
   </dl_settings>

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));

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)

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) { \
+#define NavFollow(_ac_id, _distance, _height) \
-  struct ac_info_ * ac = get_ac_info(_ac_id); \
+  nav_follow(_ac_id, _distance, _height);
-  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 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
+/** \brief Computes square distance to the HOME waypoint potentially sets
- *  verify uav is not too far away from Home.
+ * \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 
+ *  \brief Failsafe navigation without position estimation
- */
+ * 
-/** Don't navigate anymore. It's impossible without GPS.
+ * Just set attitude and throttle to FAILSAFE values
- *	Just set attitude and throttle to failsafe values
+ * to prevent the plane from crashing.
- *	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,
+  struct point c2 = { 
-		      waypoints[target].y - d*u_y,
+    waypoints[target].x - d*u_x,
-		      alt };
+    waypoints[target].y - d*u_y,
+    alt };
 
-
+  struct point c1_in = {
-  struct point c1_in = { waypoints[c1].x + radius * -u_y,
+    waypoints[c1].x + radius * -u_y,
-		       waypoints[c1].y + radius * u_x,
+    waypoints[c1].y + radius * u_x,
-		       alt  };
+    alt };
-  struct point c1_out = { waypoints[c1].x - radius * -u_y,
+  struct point c1_out = {
-		       waypoints[c1].y - radius * u_x,
+    waypoints[c1].x - radius * -u_y,
-		       alt  };
+    waypoints[c1].y - radius * u_x,
+    alt };
   
-  struct point c2_in = { c2.x + radius * -u_y,
+  struct point c2_in = {
-		       c2.y + radius * u_x,
+    c2.x + radius * -u_y,
-			alt  };
+    c2.y + radius * u_x,
-  struct point c2_out = { c2.x - radius * -u_y,
+    alt };
-		       c2.y - radius * u_x,
+  struct point c2_out = {
-		       alt  };
+    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)) { 

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 */

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];
 }

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); ***/  \
   } \
 }
 

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]);
 

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 "NavAttitude(RadOfDeg(%s));\n" (parsed_attrib x "roll");
-	lprintf "h_ctl_roll_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "roll");
 	ignore (output_vmode x "" "");
 	output_cam_mode x index_of_waypoints;
 	left (); lprintf "}\n";