Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

more toward modular vi

Browse Source
This commit is contained in:
Antoine Drouin
2010-08-24 16:47:56 +00:00
parent b2de208004
commit 0ea232ed1b
9 changed files with 46 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/booz2_a7.xml
+3
View File
@@ -162,6 +162,9 @@
<define name="BOOZ2_FACE_REINJ_1" value="1024"/> <define name="BOOZ2_FACE_REINJ_1" value="1024"/>
</section> </section>
<modules main_freq="512">
<load name="vehicle_interface_datalink.xml"/>
</modules>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="lisa_l_1.0"/> <target name="ap" board="lisa_l_1.0"/>
sw/airborne/booz/booz2_navigation.c
+12 -7
View File
@@ -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 ) { 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; int32_t s_heading, c_heading;
PPRZ_ITRIG_SIN(s_heading, nav_heading); PPRZ_ITRIG_SIN(s_heading, nav_heading);
PPRZ_ITRIG_COS(c_heading, nav_heading); PPRZ_ITRIG_COS(c_heading, nav_heading);
// FIXME : scale POS to SPEED
struct Int32Vect3 delta_pos; struct Int32Vect3 delta_pos;
VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the division is really a >> */ 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; INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
waypoints[_wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[_wp].z += delta_pos.z; waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
nav_heading += heading_rate_sp / BOOZ2_NAV_FREQ; 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); 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) { bool_t nav_detect_ground(void) {
sw/airborne/booz/booz2_navigation.h
+2 -1
View File
@@ -29,7 +29,8 @@
#include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_float.h"
#define BOOZ2_NAV_FREQ 16 #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_target;
extern struct EnuCoor_i booz2_navigation_carrot; extern struct EnuCoor_i booz2_navigation_carrot;
sw/airborne/booz/guidance/booz2_guidance_h.c
+2 -12
View File
@@ -132,6 +132,7 @@ void booz2_guidance_h_mode_changed(uint8_t new_mode) {
} }
void booz2_guidance_h_read_rc(bool_t in_flight) { void booz2_guidance_h_read_rc(bool_t in_flight) {
switch ( booz2_guidance_h_mode ) { 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: case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_read_rc(in_flight); 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; break;
case BOOZ2_GUIDANCE_H_MODE_HOVER: 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); BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
break; break;
@@ -168,7 +161,7 @@ void booz2_guidance_h_read_rc(bool_t in_flight) {
default: default:
break; break;
} }
} }
@@ -383,9 +376,6 @@ static inline void booz2_guidance_h_hover_enter(void) {
INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); 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) { static inline void booz2_guidance_h_nav_enter(void) {
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
+4 -2
View File
@@ -1,7 +1,7 @@
/* /*
* $Id$ * $Id$
* *
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com> * Copyright (C) 2008-2010 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -23,7 +23,9 @@
#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H #ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H
#define 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 Int32Quat booz_stab_att_sp_quat;
extern struct Int32Eulers booz_stab_att_ref_euler; extern struct Int32Eulers booz_stab_att_ref_euler;
extern struct Int32Quat booz_stab_att_ref_quat; extern struct Int32Quat booz_stab_att_ref_quat;
sw/airborne/math/pprz_algebra_int.h
+17 -3
View File
@@ -238,11 +238,25 @@ struct Int64Vect3 {
} }
#define INT32_VECT3_NORM(n, v) { \ #define INT32_VECT3_NORM(n, v) { \
int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \ int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \
INT32_SQRT(n, n2); \ 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 * 3x3 Matrices
sw/airborne/modules/vehicle_interface/vi.h
+1
View File
@@ -31,6 +31,7 @@
#include "std.h" #include "std.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "booz/booz2_autopilot.h" #include "booz/booz2_autopilot.h"
#include "booz/booz_stabilization.h"
#include "booz/booz_guidance.h" #include "booz/booz_guidance.h"
#include "booz/booz2_navigation.h" #include "booz/booz2_navigation.h"
sw/airborne/modules/vehicle_interface/vi_datalink.c
+5 -3
View File
@@ -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.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128;
wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128; wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128;
wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128; wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128;
VECT3_BOUND_BOX(wp_speed, wp_speed_max); VECT3_BOUND_BOX(wp_speed, wp_speed, wp_speed_max);
int16_t heading_rate = BoundAbs(vi.input.h_sp.speed.z,ViMaxHeadingRate); int16_t heading_rate = vi.input.h_sp.speed.z;
navigation_update_update_wp_from_speed(wp_id , wp_speed, heading_rate); BoundAbs(heading_rate, ViMaxHeadingRate);
navigation_update_wp_from_speed(wp_id , wp_speed, heading_rate);
} }
sw/airborne/modules/vehicle_interface/vi_datalink.h
-1
View File
@@ -25,7 +25,6 @@
#include "std.h" #include "std.h"
#include "vehicle_interface/vi.h" #include "vehicle_interface/vi.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "booz/booz_guidance.h"
#ifndef VI_MAX_H_SPEED #ifndef VI_MAX_H_SPEED
#define VI_MAX_H_SPEED 4. #define VI_MAX_H_SPEED 4.