[paparazzi-commits] [5657] en route pour le nouveau FMS

[antoine drouin]

Revision: 5657
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5657
Author:   poine
Date:     2010-08-24 14:12:51 +0000 (Tue, 24 Aug 2010)
Log Message:
-----------
en route pour le nouveau FMS

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml
    paparazzi3/trunk/conf/flight_plans/basic_booz.xml
    paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
    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

Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml  2010-08-24 
13:11:19 UTC (rev 5656)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml  2010-08-24 
14:12:51 UTC (rev 5657)
@@ -19,7 +19,7 @@
 
   <command_laws>
     <set servo="ESC"        value="motors_on?ESC_HOVER:ESC_STOPPED"/>
-    <set servo="FRONT"      value=" @PITCH + @YAW + @THRUST"/>
+    <set servo="FRONT"      value=" @PITCH + @YAW + (@THRUST<<5)"/>
     <set servo="BACK"       value="address@hidden + @YAW + @THRUST"/>
     <set servo="LEFT"       value=" @ROLL  - @YAW + @THRUST"/>
     <set servo="RIGHT"      value="address@hidden  - @YAW + @THRUST"/>

Modified: paparazzi3/trunk/conf/flight_plans/basic_booz.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/basic_booz.xml   2010-08-24 13:11:19 UTC 
(rev 5656)
+++ paparazzi3/trunk/conf/flight_plans/basic_booz.xml   2010-08-24 14:12:51 UTC 
(rev 5657)
@@ -63,11 +63,15 @@
     <block name="circle">
       <circle radius="nav_radius" wp="p1"/>
     </block>
+
+<!--
     <block name="Joystick" pre_call="NavUpdateWPFromFms(WP_p1)" 
strip_button="Joystick" strip_icon="joystick.png">
       <set var="fms.enabled" value="TRUE"/>
       <call fun="NavSetWaypointHere(WP_p1)"/>
       <stay wp="p1"/>
     </block>
+-->
+
     <block name="land here" strip_button="Land Here" 
strip_icon="land-right.png">
       <call fun="NavSetWaypointHere(WP_TD)"/>
     </block>

Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-08-24 13:11:19 UTC 
(rev 5656)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-08-24 14:12:51 UTC 
(rev 5657)
@@ -19,6 +19,7 @@
     </mode>
 
     <mode name="ppm">
+      <message name="BOOZ2_CMD"           period=".05"/>
       <message name="PPM"                 period="0.5"/>
       <message name="RC"                  period="0.5"/>
       <message name="BOOZ2_RADIO_CONTROL" period="0.5"/>

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2010-08-24 13:11:19 UTC 
(rev 5656)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.c 2010-08-24 14:12:51 UTC 
(rev 5657)
@@ -71,7 +71,7 @@
 
 void booz2_autopilot_periodic(void) {
 
-  RunOnceEvery(BOOZ2_AP_NAV_RATE, nav_periodic_task());
+  RunOnceEvery(BOOZ2_NAV_PRESCALER, 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_autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2010-08-24 13:11:19 UTC 
(rev 5656)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_autopilot.h 2010-08-24 14:12:51 UTC 
(rev 5657)
@@ -46,7 +46,6 @@
 #define BOOZ2_AP_MODE_HOVER_Z_HOLD      11
 #define BOOZ2_AP_MODE_NAV               12
 
-#define BOOZ2_AP_NAV_RATE 32
 
 extern uint8_t booz2_autopilot_mode;
 extern uint8_t booz2_autopilot_mode_auto2;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-08-24 
13:11:19 UTC (rev 5656)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-08-24 
14:12:51 UTC (rev 5657)
@@ -25,6 +25,7 @@
 
 #include "booz2_navigation.h"
 
+#include "booz/booz2_debug.h"
 #include "booz_gps.h"
 #include "booz2_ins.h"
 
@@ -32,10 +33,6 @@
 #include "modules.h"
 #include "flight_plan.h"
 
-#ifdef USE_FMS
-#include "booz_fms.h"
-#endif
-
 #include "math/pprz_algebra_int.h"
 
 const uint8_t nb_waypoint = NB_WAYPOINT;
@@ -317,28 +314,20 @@
   }
 }
 
-#ifdef USE_FMS
-void nav_update_wp_from_fms(uint8_t _wp) {
-  if (fms.enabled && _wp < nb_waypoint) {
-    int32_t s_heading, c_heading;
-    PPRZ_ITRIG_SIN(s_heading, nav_heading);
-    PPRZ_ITRIG_COS(c_heading, nav_heading);
-    struct Int32Vect3 dpos;
-    dpos.x = (NavFmsMaxHSpeed * fms.input.h_sp.speed.x >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
-    dpos.y = (NavFmsMaxHSpeed * fms.input.h_sp.speed.y >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
-    dpos.z = (NavFmsMaxVSpeed * fms.input.v_sp.climb >> (INT32_SPEED_FRAC - 
INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
-    waypoints[_wp].x += (s_heading * dpos.x + c_heading * dpos.y) >> 
INT32_TRIG_FRAC;
-    waypoints[_wp].y += (c_heading * dpos.x - s_heading * dpos.y) >> 
INT32_TRIG_FRAC;
-    waypoints[_wp].z += dpos.z;
-    int32_t dheading = (NavFmsMaxHeadingRate * fms.input.h_sp.speed.z >> 
(INT32_RATE_FRAC - INT32_ANGLE_FRAC)) / (BOOZ2_AP_NAV_RATE * 128);
-    if (dheading != 0) {
-      nav_heading += dheading;
-      INT32_COURSE_NORMALIZE(nav_heading);
-    }
-    RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, 
&(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z)));
-  }
+void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, 
int16_t heading_rate_sp ) {
+  MY_ASSERT(_wp < nb_waypoint);
+  int32_t s_heading, c_heading;
+  PPRZ_ITRIG_SIN(s_heading, nav_heading);
+  PPRZ_ITRIG_COS(c_heading, nav_heading);
+  struct Int32Vect3 delta_pos;
+  VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the 
division is really a >> */ 
+  waypoints[_wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> 
INT32_TRIG_FRAC;
+  waypoints[_wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> 
INT32_TRIG_FRAC;
+  waypoints[_wp].z += delta_pos.z;
+  nav_heading += heading_rate_sp / BOOZ2_NAV_FREQ;
+  INT32_COURSE_NORMALIZE(nav_heading);
+  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, 
&(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z)));
 }
-#endif /* USE_FMS */
 
 bool_t nav_detect_ground(void) {
   if (!booz2_autopilot_detect_ground) return FALSE;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2010-08-24 
13:11:19 UTC (rev 5656)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2010-08-24 
14:12:51 UTC (rev 5657)
@@ -28,6 +28,9 @@
 #include "math/pprz_geodetic_int.h"
 #include "math/pprz_geodetic_float.h"
 
+#define BOOZ2_NAV_FREQ 16
+#define BOOZ2_NAV_PRESCALER (PERIODIC_FREQ/BOOZ2_NAV_FREQ)
+
 extern struct EnuCoor_i booz2_navigation_target;
 extern struct EnuCoor_i booz2_navigation_carrot;
 
@@ -212,15 +215,7 @@
 #define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y)
 #define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt))
 
-#ifdef USE_FMS
-#define NavFmsMaxHSpeed ((int32_t)SPEED_BFP_OF_REAL(4.))
-#define NavFmsMaxVSpeed ((int32_t)SPEED_BFP_OF_REAL(2.))
-#define NavFmsMaxHeadingRate ((int32_t)RATE_BFP_OF_REAL(RadOfDeg(60.)))
 
-extern void nav_update_wp_from_fms(uint8_t _wp);
-#define NavUpdateWPFromFms(_wp) { \
-  nav_update_wp_from_fms(_wp); \
-}
-#endif /* USE_FMS */
+extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 
speed_sp, int16_t heading_rate_sp );
 
 #endif /* BOOZ2_NAVIGATION_H */