[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4279] use fms joystick in nav
From: |
Gautier Hattenberger |
Subject: |
[paparazzi-commits] [4279] use fms joystick in nav |
Date: |
Mon, 19 Oct 2009 13:00:35 +0000 |
Revision: 4279
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4279
Author: gautier
Date: 2009-10-19 13:00:34 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
use fms joystick in nav
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2009-10-19
12:58:38 UTC (rev 4278)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2009-10-19
13:00:34 UTC (rev 4279)
@@ -31,6 +31,8 @@
#include "booz2_autopilot.h"
#include "flight_plan.h"
+#include "booz_fms.h"
+
#include "math/pprz_algebra_int.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
@@ -294,6 +296,26 @@
}
}
+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);
+ }
+ }
+}
+
bool_t nav_detect_ground(void) {
if (!booz2_autopilot_detect_ground) return FALSE;
booz2_autopilot_detect_ground = FALSE;
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2009-10-19
12:58:38 UTC (rev 4278)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2009-10-19
13:00:34 UTC (rev 4279)
@@ -218,7 +218,14 @@
#define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y)
#define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt))
+#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 /* BOOZ2_NAVIGATION_H */
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4279] use fms joystick in nav,
Gautier Hattenberger <=