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 27 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"/>
</section>
<modules main_freq="512">
<load name="vehicle_interface_datalink.xml"/>
</modules>
<firmware name="rotorcraft">
<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 ) {
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) {
sw/airborne/booz/booz2_navigation.h
+2 -1
View File
@@ -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;
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) {
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) {
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h
+4 -2
View File
@@ -1,7 +1,7 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* 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;
sw/airborne/math/pprz_algebra_int.h
+17 -3
View File
@@ -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
sw/airborne/modules/vehicle_interface/vi.h
+1
View File
@@ -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"
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.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);
}
sw/airborne/modules/vehicle_interface/vi_datalink.h
-1
View File
@@ -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.