diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 4949dcffa7..f8858910c8 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -162,6 +162,9 @@
+
+
+
diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c
index cd89935b8f..c575d20f28 100644
--- a/sw/airborne/booz/booz2_navigation.c
+++ b/sw/airborne/booz/booz2_navigation.c
@@ -315,18 +315,23 @@ void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
}
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) {
- MY_ASSERT(_wp < nb_waypoint);
+ 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);
+ // FIXME : scale POS to SPEED
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;
+ VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the division is really a >> */
+ INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
+ 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;
+ int32_t delta_heading = heading_rate_sp / BOOZ2_NAV_FREQ;
+ delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
+ nav_heading += delta_heading;
+
INT32_COURSE_NORMALIZE(nav_heading);
- RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, &(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z)));
+ RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
}
bool_t nav_detect_ground(void) {
diff --git a/sw/airborne/booz/booz2_navigation.h b/sw/airborne/booz/booz2_navigation.h
index 44b66042ec..3754c8bf64 100644
--- a/sw/airborne/booz/booz2_navigation.h
+++ b/sw/airborne/booz/booz2_navigation.h
@@ -29,7 +29,8 @@
#include "math/pprz_geodetic_float.h"
#define BOOZ2_NAV_FREQ 16
-#define BOOZ2_NAV_PRESCALER (PERIODIC_FREQ/BOOZ2_NAV_FREQ)
+// FIXME use periodic FREQ
+#define BOOZ2_NAV_PRESCALER (512/BOOZ2_NAV_FREQ)
extern struct EnuCoor_i booz2_navigation_target;
extern struct EnuCoor_i booz2_navigation_carrot;
diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c
index 7b9bbdf98e..ecbbbd19d1 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_h.c
+++ b/sw/airborne/booz/guidance/booz2_guidance_h.c
@@ -132,6 +132,7 @@ void booz2_guidance_h_mode_changed(uint8_t new_mode) {
}
+
void booz2_guidance_h_read_rc(bool_t in_flight) {
switch ( booz2_guidance_h_mode ) {
@@ -142,17 +143,9 @@ void booz2_guidance_h_read_rc(bool_t in_flight) {
case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_read_rc(in_flight);
-#ifdef USE_FMS
- if (fms.enabled && fms.input.h_mode == BOOZ2_GUIDANCE_H_MODE_ATTITUDE)
- BOOZ_STABILIZATION_ATTITUDE_ADD_SP(fms.input.h_sp.attitude);
-#endif
break;
case BOOZ2_GUIDANCE_H_MODE_HOVER:
-#ifdef USE_FMS
- if (fms.enabled && fms.input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER)
- BOOZ2_FMS_SET_POS_SP(booz2_guidance_h_pos_sp,booz_stabilization_att_sp.psi);
-#endif
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
break;
@@ -168,7 +161,7 @@ void booz2_guidance_h_read_rc(bool_t in_flight) {
default:
break;
}
-
+
}
@@ -383,9 +376,6 @@ static inline void booz2_guidance_h_hover_enter(void) {
INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
-#ifdef USE_FMS
- BOOZ2_FMS_POS_INIT(booz2_guidance_h_pos_sp,booz2_guidance_h_rc_sp.psi);
-#endif
}
static inline void booz2_guidance_h_nav_enter(void) {
diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
index 0d14d04454..4fcb5506b9 100644
--- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
+++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2008-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -23,7 +23,9 @@
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H
#define BOOZ_STABILISATION_ATTITUDE_REF_INT_H
-extern struct Int32Eulers booz_stab_att_sp_euler;
+extern struct Int32Eulers booz_stab_att_sp_vi_euler; /* vehicle interface */
+extern struct Int32Eulers booz_stab_att_sp_rc_euler; /* radio control */
+extern struct Int32Eulers booz_stab_att_sp_euler; /* sum of the above */
extern struct Int32Quat booz_stab_att_sp_quat;
extern struct Int32Eulers booz_stab_att_ref_euler;
extern struct Int32Quat booz_stab_att_ref_quat;
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 443f543c27..82c92c46f2 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -238,11 +238,25 @@ struct Int64Vect3 {
}
-#define INT32_VECT3_NORM(n, v) { \
- int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \
- INT32_SQRT(n, n2); \
+#define INT32_VECT3_NORM(n, v) { \
+ int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \
+ INT32_SQRT(n, n2); \
}
+#define INT32_VECT3_RSHIFT(_o, _i, _r) { \
+ (_o).x = ((_i).x >> (_r)); \
+ (_o).y = ((_i).y >> (_r)); \
+ (_o).z = ((_i).z >> (_r)); \
+ }
+
+#define INT32_VECT3_LSHIFT(_o, _i, _l) { \
+ (_o).x = ((_i).x << (_l)); \
+ (_o).y = ((_i).y << (_l)); \
+ (_o).z = ((_i).z << (_l)); \
+ }
+
+
+
/*
* 3x3 Matrices
diff --git a/sw/airborne/modules/vehicle_interface/vi.h b/sw/airborne/modules/vehicle_interface/vi.h
index d7b166309f..5048cea659 100644
--- a/sw/airborne/modules/vehicle_interface/vi.h
+++ b/sw/airborne/modules/vehicle_interface/vi.h
@@ -31,6 +31,7 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "booz/booz2_autopilot.h"
+#include "booz/booz_stabilization.h"
#include "booz/booz_guidance.h"
#include "booz/booz2_navigation.h"
diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.c b/sw/airborne/modules/vehicle_interface/vi_datalink.c
index 7ad7c5339f..9cb7b8d4dd 100644
--- a/sw/airborne/modules/vehicle_interface/vi_datalink.c
+++ b/sw/airborne/modules/vehicle_interface/vi_datalink.c
@@ -43,7 +43,9 @@ void vi_update_wp(uint8_t wp_id) {
wp_speed.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128;
wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128;
wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128;
- VECT3_BOUND_BOX(wp_speed, wp_speed_max);
- int16_t heading_rate = BoundAbs(vi.input.h_sp.speed.z,ViMaxHeadingRate);
- navigation_update_update_wp_from_speed(wp_id , wp_speed, heading_rate);
+ VECT3_BOUND_BOX(wp_speed, wp_speed, wp_speed_max);
+ int16_t heading_rate = vi.input.h_sp.speed.z;
+ BoundAbs(heading_rate, ViMaxHeadingRate);
+ navigation_update_wp_from_speed(wp_id , wp_speed, heading_rate);
+
}
diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h
index 4f00856599..5f5ed04e02 100644
--- a/sw/airborne/modules/vehicle_interface/vi_datalink.h
+++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h
@@ -25,7 +25,6 @@
#include "std.h"
#include "vehicle_interface/vi.h"
#include "math/pprz_algebra_int.h"
-#include "booz/booz_guidance.h"
#ifndef VI_MAX_H_SPEED
#define VI_MAX_H_SPEED 4.