[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5661] more toward modular vi
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [5661] more toward modular vi |
|
Date: |
Tue, 24 Aug 2010 16:47:57 +0000 |
Revision: 5661
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5661
Author: poine
Date: 2010-08-24 16:47:56 +0000 (Tue, 24 Aug 2010)
Log Message:
-----------
more toward modular vi
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.c
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-08-24 16:33:19 UTC
(rev 5660)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-08-24 16:47:56 UTC
(rev 5661)
@@ -162,6 +162,9 @@
<define name="BOOZ2_FACE_REINJ_1" value="1024"/>
</section>
+ <modules main_freq="512">
+ <load name="vehicle_interface_datalink.xml"/>
+ </modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_l_1.0"/>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-08-24
16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-08-24
16:47:56 UTC (rev 5661)
@@ -315,18 +315,23 @@
}
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) {
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-08-24
16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-08-24
16:47:56 UTC (rev 5661)
@@ -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;
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-08-24 16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-08-24 16:47:56 UTC (rev 5661)
@@ -132,6 +132,7 @@
}
+
void booz2_guidance_h_read_rc(bool_t in_flight) {
switch ( booz2_guidance_h_mode ) {
@@ -142,17 +143,9 @@
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 @@
default:
break;
}
-
+
}
@@ -383,9 +376,6 @@
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) {
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
2010-08-24 16:33:19 UTC (rev 5660)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
2010-08-24 16:47:56 UTC (rev 5661)
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * 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;
Modified: paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h 2010-08-24
16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/math/pprz_algebra_int.h 2010-08-24
16:47:56 UTC (rev 5661)
@@ -238,12 +238,26 @@
}
-#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
*/
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-08-24
16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-08-24
16:47:56 UTC (rev 5661)
@@ -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"
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.c
2010-08-24 16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.c
2010-08-24 16:47:56 UTC (rev 5661)
@@ -43,7 +43,9 @@
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);
+
}
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
2010-08-24 16:33:19 UTC (rev 5660)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
2010-08-24 16:47:56 UTC (rev 5661)
@@ -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.
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5661] more toward modular vi,
antoine drouin <=