[paparazzi-commits] [4156] add ball drop

[Felix Ruess]

Revision: 4156
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4156
Author:   flixr
Date:     2009-09-12 11:27:19 +0000 (Sat, 12 Sep 2009)
Log Message:
-----------
add ball drop

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h
    paparazzi3/trunk/sw/airborne/booz/booz2_cam.c
    paparazzi3/trunk/sw/airborne/booz/booz2_main.c
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
    paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/booz/booz_drop.c
    paparazzi3/trunk/sw/airborne/booz/booz_drop.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2009-09-12 11:27:10 UTC 
(rev 4155)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2009-09-12 11:27:19 UTC 
(rev 4156)
@@ -99,7 +99,9 @@
   else { LED_ON(POWER_SWITCH_LED); } \
 }
 
+#ifndef TRESHOLD_GROUND_DETECT
 #define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
+#endif
 #define BoozDetectGroundEvent() {                                      \
     if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || 
booz2_autopilot_detect_ground_once) { \
       if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_cam.c       2009-09-12 11:27:10 UTC 
(rev 4155)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_cam.c       2009-09-12 11:27:19 UTC 
(rev 4156)
@@ -52,7 +52,6 @@
 #endif
 
 void booz2_cam_init(void) {
-  booz2_pwm_init_hw();
   booz2_cam_mode = BOOZ2_CAM_MODE_NONE;
 #ifdef BOOZ2_CAM_TILT_NEUTRAL
   booz2_cam_tilt_pwm = BOOZ2_CAM_TILT_NEUTRAL;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-12 11:27:10 UTC 
(rev 4155)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-12 11:27:19 UTC 
(rev 4156)
@@ -55,6 +55,14 @@
 #include "booz2_cam.h"
 #endif
 
+#ifdef USE_DROP
+#include "booz_drop.h"
+#endif
+
+#if defined USE_CAM || USE_DROP
+#include "booz2_pwm_hw.h"
+#endif
+
 #ifdef BOOZ2_SONAR
 #include "booz2_sonar.h"
 #endif
@@ -100,6 +108,9 @@
 
   booz2_analog_init();
   booz2_analog_baro_init();
+#if defined USE_CAM || USE_DROP
+  booz2_pwm_init_hw();
+#endif
   booz2_battery_init();
   booz_imu_init();
 
@@ -124,6 +135,10 @@
   booz2_cam_init();
 #endif
 
+#ifdef USE_DROP
+  booz_drop_init();
+#endif
+
 #ifdef BOOZ2_SONAR
   booz2_sonar_init();
 #endif
@@ -161,19 +176,19 @@
        booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE);               \
     },                                                                 \
     {                                                                  \
-      Booz2TelemetryPeriodic();                                                
\
+      Booz2TelemetryPeriodic();         \
     },                                                                 \
     {                                                                  \
-      booz_fms_periodic();                                             \
+      booz_fms_periodic();              \
     },                                                                 \
     {                                                                  \
       /*BoozControlSurfacesSetFromCommands();*/                                
\
     },                                                                 \
+    {},                                 \
     {},                                                                        
\
     {},                                                                        
\
     {},                                                                        
\
     {},                                                                        
\
-    {},                                                                        
\
     {}                                                                 \
     );                                                                 \
 
@@ -181,6 +196,10 @@
   RunOnceEvery(50,booz2_cam_periodic());
 #endif
 
+#ifdef USE_DROP
+  RunOnceEvery(50,booz_drop_periodic());
+#endif
+
 #ifdef BOOZ2_SONAR
   booz2_analog_periodic();
 #endif

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-12 
11:27:10 UTC (rev 4155)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-12 
11:27:19 UTC (rev 4156)
@@ -78,6 +78,13 @@
 
 void nav_home(void);
 
+#ifdef USE_DROP
+#include "booz_drop.h"
+#define NavDropNow() ({ booz_drop_ball = TRUE; FALSE; })
+#else
+#define NavDropNow() {}
+#endif
+
 #define NavKillThrottle() ({ kill_throttle = 1; booz2_autopilot_motors_on = 0; 
FALSE; })
 #define NavResurrect() ({ kill_throttle = 0; booz2_autopilot_motors_on = 1; 
FALSE; })
 

Added: paparazzi3/trunk/sw/airborne/booz/booz_drop.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_drop.c                               
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/booz_drop.c       2009-09-12 11:27:19 UTC 
(rev 4156)
@@ -0,0 +1,22 @@
+#include "booz_drop.h"
+#include "booz2_pwm_hw.h"
+
+
+bool_t booz_drop_ball;
+int16_t booz_drop_servo;
+
+#define DROP_SERVO_OPEN 1700
+#define DROP_SERVO_CLOSED 900
+
+void booz_drop_init(void) {
+  booz_drop_ball = FALSE;
+  booz_drop_periodic();
+}
+
+void booz_drop_periodic(void) {
+  if (booz_drop_ball == TRUE)
+    booz_drop_servo = DROP_SERVO_OPEN;
+  else
+    booz_drop_servo = DROP_SERVO_CLOSED;
+  Booz2SetPwmValue(booz_drop_servo);
+}

Added: paparazzi3/trunk/sw/airborne/booz/booz_drop.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_drop.h                               
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/booz_drop.h       2009-09-12 11:27:19 UTC 
(rev 4156)
@@ -0,0 +1,11 @@
+#ifndef BOOZ_DROP_H
+#define BOOZ_DROP_H
+
+#include "std.h"
+
+extern bool_t booz_drop_ball;
+
+extern void booz_drop_init(void);
+extern void booz_drop_periodic(void);
+
+#endif  /* BOOZ_DROP_H */

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-09-12 
11:27:10 UTC (rev 4155)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-09-12 
11:27:19 UTC (rev 4156)
@@ -48,10 +48,10 @@
 #define Q       ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
 #define Qdotdot ACCEL_NOISE*DT_HFILTER
 //TODO: proper measurement noise
-#define R_POS   7.
-#define R_POS_MIN 2.
+#define R_POS   8.
+#define R_POS_MIN 3.
 #define R_SPEED 2.
-#define R_SPEED_MIN 0.2
+#define R_SPEED_MIN 1.
 
 float Rpos, Rspeed;