[paparazzi-commits] [4059] navigation function in guidance; frequency set at 16 Hz

[Gautier Hattenberger]

Revision: 4059
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4059
Author:   gautier
Date:     2009-09-03 11:14:21 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
navigation function in guidance; frequency set at 16 Hz

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2009-09-03 11:10:18 UTC 
(rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2009-09-03 11:14:21 UTC 
(rev 4059)
@@ -59,7 +59,7 @@
 
 void booz2_autopilot_periodic(void) {
 
-  RunOnceEvery(50, nav_periodic_task_10Hz());
+  RunOnceEvery(32, nav_periodic_task());
 #ifdef BOOZ_FAILSAFE_GROUND_DETECT
   if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE && 
booz2_autopilot_detect_ground) {
     booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL);

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-03 
11:10:18 UTC (rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-03 
11:14:21 UTC (rev 4059)
@@ -25,14 +25,17 @@
 
 #include "booz2_navigation.h"
 
+#include "booz2_gps.h"
 #include "booz2_ins.h"
 
+#include "booz2_autopilot.h"
 #include "flight_plan.h"
 
 #include "math/pprz_algebra_int.h"
 
 const uint8_t nb_waypoint = NB_WAYPOINT;
-struct EnuCoor_i waypoints[NB_WAYPOINT] = WAYPOINTS_INT32;
+struct EnuCoor_f waypoints_float[NB_WAYPOINT] = WAYPOINTS;
+struct EnuCoor_i waypoints[NB_WAYPOINT];
 
 struct EnuCoor_i booz2_navigation_target;
 struct EnuCoor_i booz2_navigation_carrot;
@@ -62,15 +65,22 @@
 
 #define CLOSE_TO_WAYPOINT (15 << 8)
 #define ARRIVED_AT_WAYPOINT (10 << 8)
-#define CARROT_DIST (10 << 8)
+#define CARROT_DIST (12 << 8)
 
 void booz2_nav_init(void) {
+  // init int32 waypoints
+  uint8_t i = 0;
+  for (i = 0; i < nb_waypoint; i++) {
+    waypoints[i].x = POS_BFP_OF_REAL(waypoints_float[i].x);
+    waypoints[i].y = POS_BFP_OF_REAL(waypoints_float[i].y);
+    waypoints[i].z = POS_BFP_OF_REAL((waypoints_float[i].z - GROUND_ALT));
+  }
   nav_block = 0;
   nav_stage = 0;
-  ground_alt = (int32_t)(GROUND_ALT * 100); // cm
+  ground_alt = POS_BFP_OF_REAL(GROUND_ALT);
   nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
   nav_flight_altitude = nav_altitude;
-  flight_altitude = SECURITY_HEIGHT;
+  flight_altitude = SECURITY_ALT;
   INT32_VECT3_COPY( booz2_navigation_target, waypoints[WP_HOME]);
   INT32_VECT3_COPY( booz2_navigation_carrot, waypoints[WP_HOME]);
 
@@ -147,12 +157,16 @@
 bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
   int32_t dist_to_point;
   struct Int32Vect2 diff;
+  static uint8_t time_at_wp = 0;
   VECT2_DIFF(diff, waypoints[wp_idx], booz_ins_enu_pos);
   INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
   INT32_VECT2_NORM(dist_to_point, diff);
   //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
   //fflush(stdout);
-  if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
+  //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
+  if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
+  else time_at_wp = 0;
+  if (time_at_wp > 20) return TRUE;
   if (from_idx > 0 && from_idx < NB_WAYPOINT) {
     struct Int32Vect2 from_diff;
     VECT2_DIFF(from_diff, waypoints[wp_idx],waypoints[from_idx]);
@@ -162,8 +176,6 @@
   else return FALSE;
 }
 
-static int32_t previous_ground_alt;
-
 static inline void nav_set_altitude( void ) {
   static int32_t last_nav_alt = 0;
   if (nav_altitude != last_nav_alt) {
@@ -176,25 +188,14 @@
 unit_t nav_reset_reference( void ) {
   booz_ins_ltp_initialised = FALSE;
   booz_ins_vff_realign = TRUE;
-  previous_ground_alt = ground_alt;
   return 0;
 }
 
 unit_t nav_reset_alt( void ) {
   booz_ins_vff_realign = TRUE;
-  previous_ground_alt = ground_alt;
   return 0;
 }
 
-/** Shift altitude of the waypoint according to a new ground altitude */
-unit_t nav_update_waypoints_alt( void ) {
-//  uint8_t i;
-//  for(i = 0; i < NB_WAYPOINT; i++) {
-//    waypoints[i].z -= ground_alt - previous_ground_alt;
-//  }
-  return 0;
-}
-
 void nav_init_stage( void ) {
   INT32_VECT3_COPY(nav_last_point, booz_ins_enu_pos);
   stage_time = 0;
@@ -218,8 +219,8 @@
 }
 
 #include <stdio.h>
-void nav_periodic_task_10Hz() {
-  RunOnceEvery(10, { stage_time++;  block_time++; });
+void nav_periodic_task() {
+  RunOnceEvery(16, { stage_time++;  block_time++; });
 
   /* from flight_plan.h */
   auto_nav();
@@ -227,7 +228,7 @@
   /* run carrot loop */
   booz2_nav_run();
 
-  ground_alt = booz_ins_ltp_def.lla.alt;
+  ground_alt = POS_BFP_OF_REAL((float)booz_ins_ltp_def.hmsl / 100.);
 
 }
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-03 
11:10:18 UTC (rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-03 
11:14:21 UTC (rev 4059)
@@ -26,10 +26,12 @@
 
 #include "std.h"
 #include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
 
 extern struct EnuCoor_i booz2_navigation_target;
 extern struct EnuCoor_i booz2_navigation_carrot;
 
+extern struct EnuCoor_f waypoints_float[];
 extern struct EnuCoor_i waypoints[];
 extern const uint8_t nb_waypoint;
 
@@ -67,12 +69,14 @@
 void compute_dist2_to_home(void);
 unit_t nav_reset_reference( void ) __attribute__ ((unused));
 unit_t nav_reset_alt( void ) __attribute__ ((unused));
-unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused));
-void nav_periodic_task_10Hz(void);
+void nav_periodic_task(void);
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
 
 void nav_home(void);
 
+#define NavKillThrottle() { kill_throttle = 1; booz2_autopilot_motors_on = 0; }
+#define NavResurrect() { kill_throttle = 0; booz2_autopilot_motors_on = 1; }
+
 #define InitStage() nav_init_stage();
 
 #define Block(x) case x: nav_block=x;
@@ -97,8 +101,8 @@
 /** Time in s since the entrance in the current block */
 #define NavBlockTime() (block_time)
 
-#define NavSetGroundReferenceHere() ({ nav_reset_reference(); 
nav_update_waypoints_alt(); FALSE; })
-#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); 
nav_update_waypoints_alt(); FALSE; })
+#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
+#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
 
 #define NavSetWaypointHere(_wp) { FALSE; }
 
@@ -117,9 +121,10 @@
 #define NavQdrCloseTo(x) {}
 #define NavCourseCloseTo(x) {}
 
-#define WaypointX(_wp) (waypoints[_wp].x)
-#define WaypointY(_wp) (waypoints[_wp].y)
-#define WaypointAlt(_wp) (waypoints[_wp].z)
+#define WaypointX(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].x)
+#define WaypointY(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].y)
+#define WaypointAlt(_wp)  POS_FLOAT_OF_BFP(waypoints[_wp].z)
+#define Height(_h) (_h)
 
 /*********** Navigation to  waypoint *************************************/
 #define NavGotoWaypoint(_wp) { \
@@ -155,7 +160,7 @@
  setpoint and climb pre-command. */
 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
   vertical_mode = VERTICAL_MODE_ALT; \
-  nav_altitude = _alt; \
+  nav_altitude = POS_BFP_OF_REAL(_alt); \
 }
 
 /** Set the vertical mode to climb control with the specified climb setpoint */
@@ -177,6 +182,9 @@
   nav_roll = ANGLE_BFP_OF_REAL(_roll); \
 }
 
+#define NAV_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
+#define NavDetectGround() ( booz_ins_enu_accel.z < -NAV_GROUND_DETECT || 
booz_ins_enu_accel.z > NAV_GROUND_DETECT )
+
 #define nav_IncreaseShift(x) {}
 
 #define nav_SetNavRadius(x) {}
@@ -187,7 +195,7 @@
 
 #define booz2_navigation_SetFlightAltitude(x) { \
   flight_altitude = x; \
-  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude); \
+  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \
 }
 
 #endif /* BOOZ2_NAVIGATION_H */

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2009-09-03 11:10:18 UTC (rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2009-09-03 11:14:21 UTC (rev 4059)
@@ -39,6 +39,7 @@
 struct Int32Vect2 booz2_guidance_h_pos_err;
 struct Int32Vect2 booz2_guidance_h_speed_err;
 struct Int32Vect2 booz2_guidance_h_pos_err_sum;
+struct Int32Vect2 booz2_guidance_h_nav_err;
 
 struct Int32Eulers booz2_guidance_h_rc_sp;
 struct Int32Vect2  booz2_guidance_h_command_earth;
@@ -48,9 +49,19 @@
 int32_t booz2_guidance_h_pgain;
 int32_t booz2_guidance_h_dgain;
 int32_t booz2_guidance_h_igain;
+int32_t booz2_guidance_h_ngain;
+int32_t booz2_guidance_h_again;
 
+#ifndef BOOZ2_GUIDANCE_H_NGAIN
+#define BOOZ2_GUIDANCE_H_NGAIN 0
+#endif
 
+#ifndef BOOZ2_GUIDANCE_H_AGAIN
+#define BOOZ2_GUIDANCE_H_AGAIN 0
+#endif
+
 static inline void booz2_guidance_h_hover_run(void);
+static inline void booz2_guidance_h_nav_run(bool_t in_flight);
 static inline void booz2_guidance_h_hover_enter(void);
 static inline void booz2_guidance_h_nav_enter(void);
 
@@ -66,6 +77,8 @@
   booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN;
   booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN;
   booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN;
+  booz2_guidance_h_ngain = BOOZ2_GUIDANCE_H_NGAIN;
+  booz2_guidance_h_again = BOOZ2_GUIDANCE_H_AGAIN;
 
 }
 
@@ -111,7 +124,7 @@
   case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
     booz_stabilization_attitude_read_rc(in_flight);
 #ifdef USE_FMS
-    if (fms.enabled)
+    if (fms.enabled && fms.input.h_mode == BOOZ2_GUIDANCE_H_MODE_ATTITUDE)
       BOOZ_STABILIZATION_ATTITUDE_ADD_SP(fms.input.h_sp.attitude);
 #endif
     break;
@@ -152,15 +165,18 @@
   case BOOZ2_GUIDANCE_H_MODE_NAV:
     {
       if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
-        booz_stab_att_sp_euler.phi = 0;
-        booz_stab_att_sp_euler.theta = 0;
+#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
+        booz_stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC - 
INT32_ANGLE_FRAC);
+        booz_stab_att_sp_euler.theta = nav_pitch << (REF_ANGLE_FRAC - 
INT32_ANGLE_FRAC);
+#endif
       }
       else {
         INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, 
booz2_navigation_carrot);
 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
         booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - 
INT32_ANGLE_FRAC));
 #endif
-        booz2_guidance_h_hover_run();
+        //booz2_guidance_h_hover_run();
+        booz2_guidance_h_nav_run(in_flight);
       }
       booz_stabilization_attitude_run(in_flight);
       break;
@@ -234,6 +250,85 @@
 
 }
 
+// 20 degres -> 367002 (0.35 << 20)
+#define NAV_MAX_BANK BFP_OF_REAL(0.35,REF_ANGLE_FRAC)
+#define HOLD_DISTANCE POS_BFP_OF_REAL(10.)
+
+static inline void  booz2_guidance_h_nav_run(bool_t in_flight) {
+
+  /* compute position error    */
+  VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, 
booz2_guidance_h_pos_sp);
+  /* saturate it               */
+  VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+  /* compute speed error    */
+  VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
+  /* saturate it               */
+  VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+  
+  int32_t dist;
+  INT32_VECT2_NORM(dist, booz2_guidance_h_pos_err);
+  if ( dist < HOLD_DISTANCE ) { // Hold position
+    /* update pos error integral */
+    VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+    /* saturate it               */
+    VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, 
MAX_POS_ERR_SUM);
+  }
+  else { // Tracking algorithm, no integral
+    int32_t vect_prod = 0;
+    int32_t scal_prod = booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.x + 
booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.y;
+    // compute vectorial product only if angle < pi/2 (scalar product > 0)
+    if (scal_prod >= 0) {
+      vect_prod = ((booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.y) >> 
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10))
+                - ((booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.x) >> 
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10));
+    }
+    // multiply by vector orthogonal to speed
+    VECT2_ASSIGN(booz2_guidance_h_nav_err,
+        vect_prod * (-booz_ins_ltp_speed.y),
+        vect_prod * booz_ins_ltp_speed.x);
+    // divide by 2 times dist ( >> 16 )
+    VECT2_SDIV(booz2_guidance_h_nav_err, booz2_guidance_h_nav_err, dist*dist);
+    // *2 ??
+  }
+  if (!in_flight) { INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); }
+
+  /* run PID */
+  booz2_guidance_h_command_earth.x =
+    booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.x << (10 - 
INT32_POS_FRAC)) +
+    booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >> 
(INT32_SPEED_FRAC - 10)) +
+    booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 + 
INT32_POS_FRAC - 10)) +
+    booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x +
+    booz2_guidance_h_again * booz_ins_ltp_accel.x;
+  booz2_guidance_h_command_earth.y =
+    booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 - 
INT32_POS_FRAC)) +
+    booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >> 
(INT32_SPEED_FRAC - 10)) +
+    booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 + 
INT32_POS_FRAC - 10)) +
+    booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y +
+    booz2_guidance_h_again * booz_ins_ltp_accel.y;
+
+  VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
+  INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth, 
booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for 
trigo operation
+
+  /* Rotate to body frame */
+  int32_t s_psi, c_psi;
+  PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);      
+  PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);      
+
+  // Restore angle ref resolution after rotation
+  booz2_guidance_h_command_body.phi = 
+      ( - s_psi * booz2_guidance_h_command_earth.x + c_psi * 
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+  booz2_guidance_h_command_body.theta = 
+    - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * 
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+
+  booz2_guidance_h_command_body.phi   += booz2_guidance_h_rc_sp.phi;
+  booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
+  booz2_guidance_h_command_body.psi    = booz2_guidance_h_psi_sp + 
booz2_guidance_h_rc_sp.psi;
+  ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
+
+  EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
+
+}
+
 static inline void booz2_guidance_h_hover_enter(void) {
 
   VECT2_COPY(booz2_guidance_h_pos_sp, booz_ins_ltp_pos);

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h       
2009-09-03 11:10:18 UTC (rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h       
2009-09-03 11:14:21 UTC (rev 4059)
@@ -44,6 +44,7 @@
 extern struct Int32Vect2 booz2_guidance_h_pos_err;
 extern struct Int32Vect2 booz2_guidance_h_speed_err;
 extern struct Int32Vect2 booz2_guidance_h_pos_err_sum;
+extern struct Int32Vect2 booz2_guidance_h_nav_err;
 
 extern struct Int32Eulers booz2_guidance_h_rc_sp;
 extern struct Int32Vect2 booz2_guidance_h_command_earth;
@@ -52,6 +53,8 @@
 extern int32_t booz2_guidance_h_pgain;
 extern int32_t booz2_guidance_h_dgain;
 extern int32_t booz2_guidance_h_igain;
+extern int32_t booz2_guidance_h_ngain;
+extern int32_t booz2_guidance_h_again;
 
 
 extern void booz2_guidance_h_init(void);

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2009-09-03 11:10:18 UTC (rev 4058)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2009-09-03 11:14:21 UTC (rev 4059)
@@ -206,9 +206,11 @@
         run_hover_loop(in_flight);
       }
       else if (vertical_mode == VERTICAL_MODE_MANUAL) {
+        booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only
         booz2_guidance_v_delta_t = nav_throttle;
       }
-      booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+      //booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+    booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, 
booz2_guidance_v_delta_t);
       break;
     }
   }