Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

trajectory generation for horizontal guidance

Browse Source 
used in the navigation loop
This commit is contained in:
Gautier Hattenberger
2010-02-24 09:49:58 +00:00
parent 33f26d4ab5
commit 178d4d5037
3 changed files with 235 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/guidance/booz2_guidance_h.c
+38 -5
View File
@@ -21,6 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
#define B2_GUIDANCE_H_C
//#define B2_GUIDANCE_H_USE_REF
#include "booz2_guidance_h.h"
#include "booz_ahrs.h"
@@ -34,7 +36,10 @@
uint8_t booz2_guidance_h_mode;
struct Int32Vect2 booz2_guidance_h_pos_sp;
int32_t booz2_guidance_h_psi_sp;
int32_t booz2_guidance_h_psi_sp;
struct Int32Vect2 booz2_guidance_h_pos_ref;
struct Int32Vect2 booz2_guidance_h_speed_ref;
struct Int32Vect2 booz2_guidance_h_accel_ref;
struct Int32Vect2 booz2_guidance_h_pos_err;
struct Int32Vect2 booz2_guidance_h_speed_err;
@@ -65,6 +70,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight);
static inline void booz2_guidance_h_hover_enter(void);
static inline void booz2_guidance_h_nav_enter(void);
#define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \
b2_gh_set_ref(_pos, _speed, _accel); \
VECT2_COPY(booz2_guidance_h_pos_ref, _pos); \
VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \
VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \
}
void booz2_guidance_h_init(void) {
@@ -179,6 +191,9 @@ void booz2_guidance_h_run(bool_t in_flight) {
}
else {
INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
#ifdef B2_GUIDANCE_H_USE_REF
b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp);
#endif
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
#endif
@@ -263,13 +278,25 @@ static inline void booz2_guidance_h_hover_run(void) {
static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
/* convert our reference to generic representation */
#ifdef B2_GUIDANCE_H_USE_REF
INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
#else
VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp);
INT_VECT2_ZERO(booz2_guidance_v_speed_ref);
INT_VECT2_ZERO(booz2_guidance_v_accel_ref);
#endif
/* compute position error */
VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, booz2_guidance_h_pos_sp);
VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos, booz2_guidance_h_pos_ref);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
//VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
VECT2_DIFF(booz2_guidance_h_speed_err, booz_ins_ltp_speed, booz2_guidance_h_speed_ref);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -305,13 +332,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) +
booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - 10)) +
booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x +
booz2_guidance_h_again * booz_ins_ltp_accel.x;
booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain */
booz2_guidance_h_command_earth.y =
booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) +
booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) +
booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - 10)) +
booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y +
booz2_guidance_h_again * booz_ins_ltp_accel.y;
booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain */
VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth, booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for trigo operation
@@ -327,11 +354,13 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
booz2_guidance_h_command_body.theta =
- ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
// Add RC setpoint
booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi;
ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
// Set attitude setpoint
EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
}
@@ -352,6 +381,10 @@ static inline void booz2_guidance_h_hover_enter(void) {
static inline void booz2_guidance_h_nav_enter(void) {
INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
struct Int32Vect2 pos,speed,zero;
VECT2_COPY(pos, booz_ins_ltp_pos);
VECT2_COPY(speed, booz_ins_ltp_speed);
Booz2GuidanceHSetRef(pos, speed, zero);
struct Int32Eulers tmp_sp;
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
sw/airborne/booz/guidance/booz2_guidance_h.h
+5
View File
@@ -27,6 +27,8 @@
#include "math/pprz_algebra_int.h"
#include "booz2_guidance_h_ref.h"
#define BOOZ2_GUIDANCE_H_MODE_KILL 0
#define BOOZ2_GUIDANCE_H_MODE_RATE 1
#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2
@@ -40,6 +42,9 @@ extern uint8_t booz2_guidance_h_mode;
/* Q_int32_xx_8 */
extern struct Int32Vect2 booz2_guidance_h_pos_sp;
extern int32_t booz2_guidance_h_psi_sp;
extern struct Int32Vect2 booz2_guidance_h_pos_ref;
extern struct Int32Vect2 booz2_guidance_h_speed_ref;
extern struct Int32Vect2 booz2_guidance_h_accel_ref;
extern struct Int32Vect2 booz2_guidance_h_pos_err;
extern struct Int32Vect2 booz2_guidance_h_speed_err;
sw/airborne/booz/guidance/booz2_guidance_h_ref.h
+192
View File
@@ -0,0 +1,192 @@
/*
* $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $
*
* Copyright (C) 2008-2009 ENAC <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_GUIDANCE_H_REF_H
#define BOOZ2_GUIDANCE_H_REF_H
#include "airframe.h"
#include "inttypes.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
/* update frequency */
#define B2_GH_FREQ_FRAC 9
#define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
/* reference model accel in meters/sec2 (output) */
/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
/* int32_4_8_t */
extern struct Int32Vect2 b2_gh_accel_ref;
#define B2_GH_ACCEL_REF_FRAC 8
/* reference model speed in meters/sec (output) */
/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
extern struct Int32Vect2 b2_gh_speed_ref;
#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
/* reference model position in meters (output) */
/* Q37.26 : */
extern struct Int64Vect2 b2_gh_pos_ref;
#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
/* Saturations definition */
#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL
#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
#endif
#define B2_GH_MAX_ACCEL BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
#ifndef BOOZ2_GUIDANCE_H_REF_MAX_SPEED
#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. )
#endif
#define B2_GH_MAX_SPEED BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_SPEED, B2_GH_SPEED_REF_FRAC)
/* second order model natural frequency and damping */
#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA
#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
#endif
#ifndef BOOZ2_GUIDANCE_H_REF_ZETA
#define BOOZ2_GUIDANCE_H_REF_ZETA 0.85
#endif
#define B2_GH_ZETA_OMEGA_FRAC 10
#define B2_GH_ZETA_OMEGA BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_ZETA*BOOZ2_GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC)
#define B2_GH_OMEGA_2_FRAC 7
#define B2_GH_OMEGA_2 BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_OMEGA*BOOZ2_GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC)
/* first order time constant */
#define B2_GH_REF_THAU_F 0.5
#define B2_GH_REF_INV_THAU_FRAC 16
#define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F), B2_GH_REF_INV_THAU_FRAC)
#ifdef B2_GUIDANCE_H_C
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel);
static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
struct Int64Vect2 b2_gh_pos_ref;
struct Int32Vect2 b2_gh_speed_ref;
struct Int32Vect2 b2_gh_accel_ref;
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
struct Int64Vect2 new_pos;
new_pos.x = ((int64_t)pos.x)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
new_pos.y = ((int64_t)pos.y)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
b2_gh_pos_ref = new_pos;
INT32_VECT2_RSHIFT(b2_gh_speed_ref, speed, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
INT32_VECT2_RSHIFT(b2_gh_accel_ref, accel, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
}
static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, b2_gh_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*B2_GH_ZETA_OMEGA);
INT32_VECT2_RSHIFT(speed, speed, B2_GH_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
INT32_VECT2_RSHIFT(pos_err, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
VECT2_DIFF(pos_err, pos_err, pos_sp);
// convert to accel resolution
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - B2_GH_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
VECT2_SMUL(pos, pos_err, (-B2_GH_OMEGA_2));
INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(b2_gh_accel_ref, speed, pos);
/* Saturate accelerations */
VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
/* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x < 0)
b2_gh_accel_ref.x = 0;
}
else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x > 0)
b2_gh_accel_ref.x = 0;
}
if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y < 0)
b2_gh_accel_ref.y = 0;
}
else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y > 0)
b2_gh_accel_ref.y = 0;
}
}
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
// compute speed error
struct Int32Vect2 speed_err;
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err);
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (B2_GH_SPEED_REF_FRAC - B2_GH_ACCEL_REF_FRAC));
// compute accel from speed_sp
VECT2_SMUL(b2_gh_accel_ref, speed_err, -B2_GH_REF_INV_THAU);
INT32_VECT2_RSHIFT(b2_gh_accel_ref, b2_gh_accel_ref, B2_GH_REF_INV_THAU_FRAC);
/* Saturate accelerations */
VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
/* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x < 0)
b2_gh_accel_ref.x = 0;
}
else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x > 0)
b2_gh_accel_ref.x = 0;
}
if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y < 0)
b2_gh_accel_ref.y = 0;
}
else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y > 0)
b2_gh_accel_ref.y = 0;
}
}
#endif /* B2_GUIDANCE_H_C */
#endif /* BOOZ2_GUIDANCE_H_REF_H */