[paparazzi-commits] [4119] hysteresis on radio ok; ground detect use main event; rc not used in guidance if lost

[Gautier Hattenberger]

Revision: 4119
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4119
Author:   gautier
Date:     2009-09-09 09:08:33 +0000 (Wed, 09 Sep 2009)
Log Message:
-----------
hysteresis on radio ok; ground detect use main event; rc not used in guidance 
if lost

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2009-09-08 20:20:44 UTC 
(rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2009-09-09 09:08:33 UTC 
(rev 4119)
@@ -39,6 +39,7 @@
 bool_t kill_throttle;
 
 bool_t booz2_autopilot_detect_ground;
+bool_t booz2_autopilot_detect_ground_once;
 
 #define BOOZ2_AUTOPILOT_MOTOR_ON_TIME     40
 #define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME    40
@@ -54,6 +55,7 @@
   booz2_autopilot_in_flight_counter = 0;
   booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
   booz2_autopilot_detect_ground = FALSE;
+  booz2_autopilot_detect_ground_once = FALSE;
 }
 
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2009-09-08 20:20:44 UTC 
(rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2009-09-09 09:08:33 UTC 
(rev 4119)
@@ -56,6 +56,7 @@
 extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
 
 extern bool_t booz2_autopilot_detect_ground;
+extern bool_t booz2_autopilot_detect_ground_once;
 
 #ifndef BOOZ2_MODE_MANUAL
 #define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
@@ -88,11 +89,13 @@
 
 #define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
 #define BoozDetectGroundEvent() {                                      \
-    if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE) {              \
-      if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||            \
-         booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT)                \
-       booz2_autopilot_detect_ground = TRUE;                           \
-    }                                                                  \
+    if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || 
booz2_autopilot_detect_ground_once) { \
+      if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \
+               booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) { \
+        booz2_autopilot_detect_ground = TRUE; \
+        booz2_autopilot_detect_ground_once = FALSE; \
+      } \
+    } \
   }
 
 #endif /* BOOZ2_AUTOPILOT_H */

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-08 
20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-09 
09:08:33 UTC (rev 4119)
@@ -137,7 +137,7 @@
     struct Int32Vect2 pos_diff;
     VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_center]);
     // go back to half metric precision or values are too large
-    INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC-1);
+    //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
     // store last qdr
     int32_t last_qdr = nav_circle_qdr;
     // compute qdr
@@ -153,7 +153,7 @@
     int32_t abs_radius = abs(radius);
     // carrot_angle
     int32_t carrot_angle = (CARROT_DIST / abs_radius) << (INT32_TRIG_FRAC - 
INT32_POS_FRAC);
-    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
+    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_2);
     carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
     int32_t s_carrot, c_carrot;
     PPRZ_ITRIG_SIN(s_carrot, carrot_angle);    
@@ -182,6 +182,7 @@
   INT32_SQRT(leg_length,leg_length2);
   int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) 
/ leg_length;
   nav_leg_progress += Max((CARROT_DIST >> INT32_POS_FRAC), 0);
+  Bound(nav_leg_progress,0,leg_length);
   struct Int32Vect2 progress_pos;
   VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
   VECT2_SDIV(progress_pos, progress_pos, leg_length);
@@ -287,5 +288,11 @@
   }
 }
 
+bool_t nav_detect_ground(void) {
+  if (!booz2_autopilot_detect_ground) return FALSE;
+  booz2_autopilot_detect_ground = FALSE;
+  return TRUE;
+}
+
 void nav_home(void) {}
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-08 
20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-09 
09:08:33 UTC (rev 4119)
@@ -74,6 +74,7 @@
 unit_t nav_reset_alt( void ) __attribute__ ((unused));
 void nav_periodic_task(void);
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
+bool_t nav_detect_ground(void);
 
 void nav_home(void);
 
@@ -107,7 +108,7 @@
 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
 #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
 
-#define NavSetWaypointHere(_wp) { FALSE; }
+#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], 
booz_ins_enu_pos); FALSE; })
 
 #define WaypointX(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].x)
 #define WaypointY(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].y)
@@ -189,8 +190,8 @@
   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 NavStartDetectGround() ({ booz2_autopilot_detect_ground_once = TRUE; 
FALSE; })
+#define NavDetectGround() nav_detect_ground()
 
 #define nav_IncreaseShift(x) {}
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz_radio_control.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_radio_control.c      2009-09-08 
20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz_radio_control.c      2009-09-09 
09:08:33 UTC (rev 4119)
@@ -33,6 +33,7 @@
     radio_control.values[i] = 0;
   radio_control.status = RADIO_CONTROL_REALLY_LOST;
   radio_control.time_since_last_frame = RADIO_CONTROL_REALLY_LOST_TIME;
+  radio_control.radio_ok_cpt = 0;
   radio_control.frame_rate = 0;
   radio_control.frame_cpt = 0;
   radio_control_impl_init();
@@ -52,8 +53,10 @@
     radio_control.status = RADIO_CONTROL_REALLY_LOST;
   } 
   else {
-    if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME)
+    if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME) {
       radio_control.status = RADIO_CONTROL_LOST;
+      radio_control.radio_ok_cpt = RADIO_CONTROL_OK_CPT;
+    }
     radio_control.time_since_last_frame++;
   }
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz_radio_control.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_radio_control.h      2009-09-08 
20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/booz_radio_control.h      2009-09-09 
09:08:33 UTC (rev 4119)
@@ -40,10 +40,13 @@
 /* timeouts - for now assumes 60Hz periodic */
 #define RADIO_CONTROL_LOST_TIME        30
 #define RADIO_CONTROL_REALLY_LOST_TIME 60
+/* number of valid frames before going back to OK */
+#define RADIO_CONTROL_OK_CPT      15
 
 struct RadioControl {
   uint8_t status;
   uint8_t time_since_last_frame;
+  uint8_t radio_ok_cpt;
   uint8_t frame_rate;
   uint8_t frame_cpt;
   pprz_t  values[RADIO_CONTROL_NB_CHANNEL];

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2009-09-08 20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c       
2009-09-09 09:08:33 UTC (rev 4119)
@@ -138,8 +138,13 @@
     break;
   
   case BOOZ2_GUIDANCE_H_MODE_NAV:
-    BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
-    booz2_guidance_h_rc_sp.psi = 0;
+    if (radio_control.status == RADIO_CONTROL_OK) {
+      BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+      booz2_guidance_h_rc_sp.psi = 0;
+    }
+    else {
+      INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
+    }
     break;
   }
 
@@ -164,6 +169,8 @@
     
   case BOOZ2_GUIDANCE_H_MODE_NAV:
     {
+      if (!in_flight) booz2_guidance_h_nav_enter();
+
       if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
         booz_stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC - 
INT32_ANGLE_FRAC);

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2009-09-08 20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2009-09-09 09:08:33 UTC (rev 4119)
@@ -209,8 +209,11 @@
         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] = Min( booz2_guidance_v_rc_delta_t, 
booz2_guidance_v_delta_t);
+      /* use rc limitation if available */
+      if (radio_control.status == RADIO_CONTROL_OK)
+        booz_stabilization_cmd[COMMAND_THRUST] = Min( 
booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
+      else
+        booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
       break;
     }
   }

Modified: 
paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_ppm.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_ppm.h    
2009-09-08 20:20:44 UTC (rev 4118)
+++ paparazzi3/trunk/sw/airborne/booz/radio_control/booz_radio_control_ppm.h    
2009-09-09 09:08:33 UTC (rev 4119)
@@ -51,9 +51,12 @@
     if (booz_radio_control_ppm_frame_available) {                      \
       radio_control.frame_cpt++;                                       \
       radio_control.time_since_last_frame = 0;                         \
-      radio_control.status = RADIO_CONTROL_OK;                         \
-      NormalizePpm();                                                  \
-      _received_frame_handler();                                       \
+      if (radio_control.radio_ok_cpt > 0) radio_control.radio_ok_cpt--; \
+      else { \
+        radio_control.status = RADIO_CONTROL_OK;                               
\
+        NormalizePpm();                                                        
\
+        _received_frame_handler();                                     \
+      } \
       booz_radio_control_ppm_frame_available = FALSE;                  \
     }                                                                  \
   }