mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 15:30:08 +08:00
[guidance] split h_ref and v_ref into h and c files
This commit is contained in:
Gautier Hattenberger
@@ -24,8 +24,6 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#define GUIDANCE_H_C
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
@@ -84,7 +82,7 @@ static inline void guidance_h_hover_enter(void);
|
||||
static inline void guidance_h_nav_enter(void);
|
||||
|
||||
#define GuidanceHSetRef(_pos, _speed, _accel) { \
|
||||
b2_gh_set_ref(_pos, _speed, _accel); \
|
||||
gh_set_ref(_pos, _speed, _accel); \
|
||||
VECT2_COPY(guidance_h_pos_ref, _pos); \
|
||||
VECT2_COPY(guidance_h_speed_ref, _speed); \
|
||||
VECT2_COPY(guidance_h_accel_ref, _accel); \
|
||||
@@ -242,10 +240,10 @@ void guidance_h_run(bool_t in_flight) {
|
||||
static inline void guidance_h_update_reference(bool_t use_ref) {
|
||||
/* convert our reference to generic representation */
|
||||
if (use_ref) {
|
||||
b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
|
||||
INT32_VECT2_RSHIFT(guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
|
||||
INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
|
||||
INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
|
||||
gh_update_ref_from_pos_sp(guidance_h_pos_sp);
|
||||
INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
|
||||
INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
|
||||
INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
|
||||
} else {
|
||||
VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp);
|
||||
INT_VECT2_ZERO(guidance_h_speed_ref);
|
||||
|
||||
@@ -0,0 +1,244 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2013 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/** @file firmwares/rotorcraft/guidance/guidance_h_ref.c
|
||||
* Reference generation for horizontal guidance.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
/** Reference model acceleration.
|
||||
* in meters/sec2 (output)
|
||||
* fixed point representation: Q23.8
|
||||
* accuracy 0.0039, range 8388km/s2
|
||||
*/
|
||||
struct Int32Vect2 gh_accel_ref;
|
||||
|
||||
/** Reference model speed.
|
||||
* in meters/sec
|
||||
* with fixedpoint representation: Q14.17
|
||||
* accuracy 0.0000076 , range 16384m/s
|
||||
*/
|
||||
struct Int32Vect2 gh_speed_ref;
|
||||
|
||||
/* Reference model position.
|
||||
* in meters
|
||||
* with fixedpoint representation: Q37.26
|
||||
*/
|
||||
struct Int64Vect2 gh_pos_ref;
|
||||
|
||||
/** Accel saturation.
|
||||
* tanf(RadOfDeg(30.))*9.81 = 5.66
|
||||
*/
|
||||
#ifndef GUIDANCE_H_REF_MAX_ACCEL
|
||||
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
|
||||
#endif
|
||||
#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)
|
||||
|
||||
/** Speed saturation */
|
||||
#ifndef GUIDANCE_H_REF_MAX_SPEED
|
||||
#define GUIDANCE_H_REF_MAX_SPEED 5.
|
||||
#endif
|
||||
/** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */
|
||||
#define GH_MAX_SPEED_REF_FRAC 7
|
||||
#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC)
|
||||
|
||||
/** second order model natural frequency */
|
||||
#ifndef GUIDANCE_H_REF_OMEGA
|
||||
#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
|
||||
#endif
|
||||
/** second order model damping */
|
||||
#ifndef GUIDANCE_H_REF_ZETA
|
||||
#define GUIDANCE_H_REF_ZETA 0.85
|
||||
#endif
|
||||
#define GH_ZETA_OMEGA_FRAC 10
|
||||
#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC)
|
||||
#define GH_OMEGA_2_FRAC 7
|
||||
#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC)
|
||||
|
||||
/** first order time constant */
|
||||
#define GH_REF_THAU_F 0.5
|
||||
#define GH_REF_INV_THAU_FRAC 16
|
||||
#define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC)
|
||||
|
||||
static struct Int32Vect2 gh_max_speed_ref;
|
||||
static struct Int32Vect2 gh_max_accel_ref;
|
||||
|
||||
static int32_t route_ref;
|
||||
static int32_t s_route_ref;
|
||||
static int32_t c_route_ref;
|
||||
|
||||
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
|
||||
struct Int64Vect2 new_pos;
|
||||
new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
|
||||
new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
|
||||
gh_pos_ref = new_pos;
|
||||
INT32_VECT2_RSHIFT(gh_speed_ref, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
|
||||
INT32_VECT2_RSHIFT(gh_accel_ref, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
|
||||
}
|
||||
|
||||
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
|
||||
|
||||
VECT2_ADD(gh_pos_ref, gh_speed_ref);
|
||||
VECT2_ADD(gh_speed_ref, 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, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
|
||||
VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
|
||||
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
|
||||
// compute pos error in pos_sp resolution
|
||||
struct Int32Vect2 pos_err;
|
||||
INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (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 - GH_ACCEL_REF_FRAC));
|
||||
// compute the "pos part" of accel
|
||||
struct Int32Vect2 pos;
|
||||
VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
|
||||
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
|
||||
// sum accel
|
||||
VECT2_SUM(gh_accel_ref, speed, pos);
|
||||
|
||||
/* Compute route reference before saturation */
|
||||
// use metric precision or values are too large
|
||||
INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
|
||||
route_ref = abs(route_ref);
|
||||
/* Compute North and East route components */
|
||||
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
|
||||
PPRZ_ITRIG_COS(c_route_ref, route_ref);
|
||||
/* Compute maximum acceleration*/
|
||||
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
|
||||
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* Compute maximum speed*/
|
||||
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
|
||||
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* restore gh_speed_ref range (Q14.17) */
|
||||
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
|
||||
|
||||
/* Saturate accelerations */
|
||||
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
|
||||
gh_accel_ref.x = -gh_max_accel_ref.x;
|
||||
}
|
||||
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
|
||||
gh_accel_ref.x = gh_max_accel_ref.x;
|
||||
}
|
||||
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
|
||||
gh_accel_ref.y = -gh_max_accel_ref.y;
|
||||
}
|
||||
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
|
||||
gh_accel_ref.y = gh_max_accel_ref.y;
|
||||
}
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
|
||||
gh_speed_ref.x = -gh_max_speed_ref.x;
|
||||
if (gh_accel_ref.x < 0)
|
||||
gh_accel_ref.x = 0;
|
||||
}
|
||||
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
|
||||
gh_speed_ref.x = gh_max_speed_ref.x;
|
||||
if (gh_accel_ref.x > 0)
|
||||
gh_accel_ref.x = 0;
|
||||
}
|
||||
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
|
||||
gh_speed_ref.y = -gh_max_speed_ref.y;
|
||||
if (gh_accel_ref.y < 0)
|
||||
gh_accel_ref.y = 0;
|
||||
}
|
||||
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
|
||||
gh_speed_ref.y = gh_max_speed_ref.y;
|
||||
if (gh_accel_ref.y > 0)
|
||||
gh_accel_ref.y = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
|
||||
/* WARNING: SPEED SATURATION UNTESTED */
|
||||
VECT2_ADD(gh_pos_ref, gh_speed_ref);
|
||||
VECT2_ADD(gh_speed_ref, gh_accel_ref);
|
||||
|
||||
// compute speed error
|
||||
struct Int32Vect2 speed_err;
|
||||
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
|
||||
VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
|
||||
// convert to accel resolution
|
||||
INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
|
||||
// compute accel from speed_sp
|
||||
VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU);
|
||||
INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC);
|
||||
|
||||
/* Compute route reference before saturation */
|
||||
// use metric precision or values are too large
|
||||
INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
|
||||
route_ref = abs(route_ref);
|
||||
/* Compute North and East route components */
|
||||
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
|
||||
PPRZ_ITRIG_COS(c_route_ref, route_ref);
|
||||
|
||||
/* Compute maximum acceleration*/
|
||||
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
|
||||
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* Compute maximum speed*/
|
||||
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
|
||||
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* restore gh_speed_ref range (Q14.17) */
|
||||
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
|
||||
|
||||
/* Saturate accelerations */
|
||||
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
|
||||
gh_accel_ref.x = -gh_max_accel_ref.x;
|
||||
}
|
||||
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
|
||||
gh_accel_ref.x = gh_max_accel_ref.x;
|
||||
}
|
||||
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
|
||||
gh_accel_ref.y = -gh_max_accel_ref.y;
|
||||
}
|
||||
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
|
||||
gh_accel_ref.y = gh_max_accel_ref.y;
|
||||
}
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
|
||||
gh_speed_ref.x = -gh_max_speed_ref.x;
|
||||
if (gh_accel_ref.x < 0)
|
||||
gh_accel_ref.x = 0;
|
||||
}
|
||||
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
|
||||
gh_speed_ref.x = gh_max_speed_ref.x;
|
||||
if (gh_accel_ref.x > 0)
|
||||
gh_accel_ref.x = 0;
|
||||
}
|
||||
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
|
||||
gh_speed_ref.y = -gh_max_speed_ref.y;
|
||||
if (gh_accel_ref.y < 0)
|
||||
gh_accel_ref.y = 0;
|
||||
}
|
||||
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
|
||||
gh_speed_ref.y = gh_max_speed_ref.y;
|
||||
if (gh_accel_ref.y > 0)
|
||||
gh_accel_ref.y = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -27,244 +27,40 @@
|
||||
#ifndef GUIDANCE_H_REF_H
|
||||
#define GUIDANCE_H_REF_H
|
||||
|
||||
#include "generated/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)
|
||||
/** Update frequency
|
||||
*/
|
||||
#define GH_FREQ_FRAC 9
|
||||
#define GH_FREQ (1<<GH_FREQ_FRAC)
|
||||
|
||||
/** Reference model acceleration.
|
||||
* in meters/sec2 (output)
|
||||
* fixed point representation: Q23.8
|
||||
* accuracy 0.0039, range 8388km/s2
|
||||
*/
|
||||
extern struct Int32Vect2 b2_gh_accel_ref;
|
||||
#define B2_GH_ACCEL_REF_FRAC 8
|
||||
extern struct Int32Vect2 gh_accel_ref;
|
||||
#define GH_ACCEL_REF_FRAC 8
|
||||
|
||||
/** Reference model speed.
|
||||
* in meters/sec
|
||||
* with fixedpoint representation: Q14.17
|
||||
* accuracy 0.0000076 , range 16384m/s
|
||||
*/
|
||||
extern struct Int32Vect2 b2_gh_speed_ref;
|
||||
#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
|
||||
extern struct Int32Vect2 gh_speed_ref;
|
||||
#define GH_SPEED_REF_FRAC (GH_ACCEL_REF_FRAC + GH_FREQ_FRAC)
|
||||
|
||||
/* Reference model position.
|
||||
* in meters
|
||||
* with fixedpoint representation: Q37.26
|
||||
*/
|
||||
extern struct Int64Vect2 b2_gh_pos_ref;
|
||||
#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
|
||||
extern struct Int64Vect2 gh_pos_ref;
|
||||
#define GH_POS_REF_FRAC (GH_SPEED_REF_FRAC + GH_FREQ_FRAC)
|
||||
|
||||
/** Accel saturation.
|
||||
* tanf(RadOfDeg(30.))*9.81 = 5.66
|
||||
*/
|
||||
#ifndef GUIDANCE_H_REF_MAX_ACCEL
|
||||
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
|
||||
#endif
|
||||
#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
|
||||
|
||||
/** Speed saturation */
|
||||
#ifndef GUIDANCE_H_REF_MAX_SPEED
|
||||
#define GUIDANCE_H_REF_MAX_SPEED 5.
|
||||
#endif
|
||||
/** @todo B2_GH_MAX_SPEED must be limited to 2^14 to avoid overflow */
|
||||
#define B2_GH_MAX_SPEED_REF_FRAC 7
|
||||
#define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_MAX_SPEED_REF_FRAC)
|
||||
|
||||
/** second order model natural frequency */
|
||||
#ifndef GUIDANCE_H_REF_OMEGA
|
||||
#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
|
||||
#endif
|
||||
/** second order model damping */
|
||||
#ifndef GUIDANCE_H_REF_ZETA
|
||||
#define GUIDANCE_H_REF_ZETA 0.85
|
||||
#endif
|
||||
#define B2_GH_ZETA_OMEGA_FRAC 10
|
||||
#define B2_GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC)
|
||||
#define B2_GH_OMEGA_2_FRAC 7
|
||||
#define B2_GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*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 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 struct Int32Vect2 b2_gh_max_speed_ref;
|
||||
static struct Int32Vect2 b2_gh_max_accel_ref;
|
||||
|
||||
static int32_t route_ref;
|
||||
static int32_t s_route_ref;
|
||||
static int32_t c_route_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);
|
||||
|
||||
/* Compute route reference before saturation */
|
||||
// use metric precision or values are too large
|
||||
INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
|
||||
/* Compute North and East route components */
|
||||
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
|
||||
PPRZ_ITRIG_COS(c_route_ref, route_ref);
|
||||
c_route_ref = abs(c_route_ref);
|
||||
s_route_ref = abs(s_route_ref);
|
||||
/* Compute maximum acceleration*/
|
||||
b2_gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
|
||||
b2_gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* Compute maximum speed*/
|
||||
b2_gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
|
||||
b2_gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* restore b2_gh_speed_ref range (Q14.17) */
|
||||
INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC));
|
||||
|
||||
/* Saturate accelerations */
|
||||
if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) {
|
||||
b2_gh_accel_ref.x = -b2_gh_max_accel_ref.x;
|
||||
}
|
||||
else if (b2_gh_accel_ref.x >= b2_gh_max_accel_ref.x) {
|
||||
b2_gh_accel_ref.x = b2_gh_max_accel_ref.x;
|
||||
}
|
||||
if (b2_gh_accel_ref.y <= -b2_gh_max_accel_ref.y) {
|
||||
b2_gh_accel_ref.y = -b2_gh_max_accel_ref.y;
|
||||
}
|
||||
else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) {
|
||||
b2_gh_accel_ref.y = b2_gh_max_accel_ref.y;
|
||||
}
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) {
|
||||
b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x;
|
||||
if (b2_gh_accel_ref.x < 0)
|
||||
b2_gh_accel_ref.x = 0;
|
||||
}
|
||||
else if (b2_gh_speed_ref.x >= b2_gh_max_speed_ref.x) {
|
||||
b2_gh_speed_ref.x = b2_gh_max_speed_ref.x;
|
||||
if (b2_gh_accel_ref.x > 0)
|
||||
b2_gh_accel_ref.x = 0;
|
||||
}
|
||||
if (b2_gh_speed_ref.y <= -b2_gh_max_speed_ref.y) {
|
||||
b2_gh_speed_ref.y = -b2_gh_max_speed_ref.y;
|
||||
if (b2_gh_accel_ref.y < 0)
|
||||
b2_gh_accel_ref.y = 0;
|
||||
}
|
||||
else if (b2_gh_speed_ref.y >= b2_gh_max_speed_ref.y) {
|
||||
b2_gh_speed_ref.y = b2_gh_max_speed_ref.y;
|
||||
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) {
|
||||
/* WARNING: SPEED SATURATION UNTESTED */
|
||||
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);
|
||||
|
||||
/* Compute route reference before saturation */
|
||||
// use metric precision or values are too large
|
||||
INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
|
||||
/* Compute North and East route components */
|
||||
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
|
||||
PPRZ_ITRIG_COS(c_route_ref, route_ref);
|
||||
c_route_ref = abs(c_route_ref);
|
||||
s_route_ref = abs(s_route_ref);
|
||||
|
||||
/* Compute maximum acceleration*/
|
||||
b2_gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
|
||||
b2_gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* Compute maximum speed*/
|
||||
b2_gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
|
||||
b2_gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
|
||||
/* restore b2_gh_speed_ref range (Q14.17) */
|
||||
INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC));
|
||||
|
||||
/* Saturate accelerations */
|
||||
if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) {
|
||||
b2_gh_accel_ref.x = -b2_gh_max_accel_ref.x;
|
||||
}
|
||||
else if (b2_gh_accel_ref.x >= b2_gh_max_accel_ref.x) {
|
||||
b2_gh_accel_ref.x = b2_gh_max_accel_ref.x;
|
||||
}
|
||||
if (b2_gh_accel_ref.y <= -b2_gh_max_accel_ref.y) {
|
||||
b2_gh_accel_ref.y = -b2_gh_max_accel_ref.y;
|
||||
}
|
||||
else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) {
|
||||
b2_gh_accel_ref.y = b2_gh_max_accel_ref.y;
|
||||
}
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) {
|
||||
b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x;
|
||||
if (b2_gh_accel_ref.x < 0)
|
||||
b2_gh_accel_ref.x = 0;
|
||||
}
|
||||
else if (b2_gh_speed_ref.x >= b2_gh_max_speed_ref.x) {
|
||||
b2_gh_speed_ref.x = b2_gh_max_speed_ref.x;
|
||||
if (b2_gh_accel_ref.x > 0)
|
||||
b2_gh_accel_ref.x = 0;
|
||||
}
|
||||
if (b2_gh_speed_ref.y <= -b2_gh_max_speed_ref.y) {
|
||||
b2_gh_speed_ref.y = -b2_gh_max_speed_ref.y;
|
||||
if (b2_gh_accel_ref.y < 0)
|
||||
b2_gh_accel_ref.y = 0;
|
||||
}
|
||||
else if (b2_gh_speed_ref.y >= b2_gh_max_speed_ref.y) {
|
||||
b2_gh_speed_ref.y = b2_gh_max_speed_ref.y;
|
||||
if (b2_gh_accel_ref.y > 0)
|
||||
b2_gh_accel_ref.y = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* GUIDANCE_H_C */
|
||||
extern void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel);
|
||||
extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
|
||||
extern void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
|
||||
|
||||
#endif /* GUIDANCE_H_REF_H */
|
||||
|
||||
@@ -25,7 +25,6 @@
|
||||
*/
|
||||
|
||||
#define GUIDANCE_V_C
|
||||
#define GUIDANCE_V_USE_REF 1
|
||||
#include "firmwares/rotorcraft/guidance/guidance_v.h"
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,151 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/** @file firmwares/rotorcraft/guidance/guidance_v_ref.c
|
||||
* Reference generation for vertical guidance.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
/** reference model vertical accel in meters/s^2 (output)
|
||||
* fixed point representation with #GV_ZDD_REF_FRAC
|
||||
* Q23.8 : accuracy 0.0039 , range 8388km/s^2
|
||||
*/
|
||||
int32_t gv_zdd_ref;
|
||||
|
||||
/** reference model vertical speed in meters/sec (output)
|
||||
* fixed point representation with #GV_ZD_REF_FRAC
|
||||
* Q14.17 : accuracy 0.0000076 , range 16384m/s2
|
||||
*/
|
||||
int32_t gv_zd_ref;
|
||||
|
||||
/** reference model altitude in meters (output)
|
||||
* fixed point representation with #GV_Z_REF_FRAC
|
||||
* Q37.26 :
|
||||
*/
|
||||
int64_t gv_z_ref;
|
||||
|
||||
|
||||
/* Saturations definition */
|
||||
#ifndef GUIDANCE_V_REF_MIN_ZDD
|
||||
#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
|
||||
#endif
|
||||
#define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MAX_ZDD
|
||||
#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
|
||||
#endif
|
||||
#define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MIN_ZD
|
||||
#define GUIDANCE_V_REF_MIN_ZD (-3.)
|
||||
#endif
|
||||
#define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MAX_ZD
|
||||
#define GUIDANCE_V_REF_MAX_ZD ( 3.)
|
||||
#endif
|
||||
#define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
|
||||
|
||||
/* second order model natural frequency and damping */
|
||||
#ifndef GUIDANCE_V_REF_OMEGA
|
||||
#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
|
||||
#endif
|
||||
#ifndef GUIDANCE_V_REF_ZETA
|
||||
#define GUIDANCE_V_REF_ZETA 0.85
|
||||
#endif
|
||||
#define GV_ZETA_OMEGA_FRAC 10
|
||||
#define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
|
||||
#define GV_OMEGA_2_FRAC 7
|
||||
#define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
|
||||
|
||||
/* first order time constant */
|
||||
#define GV_REF_THAU_F 0.25
|
||||
#define GV_REF_INV_THAU_FRAC 16
|
||||
#define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)
|
||||
|
||||
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
|
||||
int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC);
|
||||
gv_z_ref = new_z;
|
||||
gv_zd_ref = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
|
||||
gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
|
||||
}
|
||||
|
||||
void gv_update_ref_from_z_sp(int32_t z_sp) {
|
||||
|
||||
gv_z_ref += gv_zd_ref;
|
||||
gv_zd_ref += gv_zdd_ref;
|
||||
|
||||
// compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
|
||||
int32_t zd_zdd_res = gv_zd_ref>>(GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC);
|
||||
int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC);
|
||||
// compute z error in z_sp resolution
|
||||
int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC));
|
||||
// convert to accel resolution
|
||||
int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC);
|
||||
int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC;
|
||||
gv_zdd_ref = zdd_speed + zdd_pos;
|
||||
|
||||
/* Saturate accelerations */
|
||||
Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gv_zd_ref <= GV_MIN_ZD) {
|
||||
gv_zd_ref = GV_MIN_ZD;
|
||||
if (gv_zdd_ref < 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
else if (gv_zd_ref >= GV_MAX_ZD) {
|
||||
gv_zd_ref = GV_MAX_ZD;
|
||||
if (gv_zdd_ref > 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void gv_update_ref_from_zd_sp(int32_t zd_sp) {
|
||||
|
||||
gv_z_ref += gv_zd_ref;
|
||||
gv_zd_ref += gv_zdd_ref;
|
||||
|
||||
int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
|
||||
int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC);
|
||||
gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC;
|
||||
|
||||
/* Saturate accelerations */
|
||||
Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gv_zd_ref <= GV_MIN_ZD) {
|
||||
gv_zd_ref = GV_MIN_ZD;
|
||||
if (gv_zdd_ref < 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
else if (gv_zd_ref >= GV_MAX_ZD) {
|
||||
gv_zd_ref = GV_MAX_ZD;
|
||||
if (gv_zdd_ref > 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -27,12 +28,12 @@
|
||||
#ifndef GUIDANCE_V_REF_H
|
||||
#define GUIDANCE_V_REF_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "inttypes.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
/* update frequency */
|
||||
/** Update frequency
|
||||
*/
|
||||
#define GV_FREQ_FRAC 9
|
||||
#define GV_FREQ (1<<GV_FREQ_FRAC)
|
||||
|
||||
@@ -63,117 +64,8 @@ extern int64_t gv_z_ref;
|
||||
/** number of bits for the fractional part of #gv_z_ref */
|
||||
#define GV_Z_REF_FRAC (GV_ZD_REF_FRAC + GV_FREQ_FRAC)
|
||||
|
||||
/* Saturations definition */
|
||||
#ifndef GUIDANCE_V_REF_MIN_ZDD
|
||||
#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
|
||||
#endif
|
||||
#define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MAX_ZDD
|
||||
#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
|
||||
#endif
|
||||
#define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MIN_ZD
|
||||
#define GUIDANCE_V_REF_MIN_ZD (-3.)
|
||||
#endif
|
||||
#define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
|
||||
|
||||
#ifndef GUIDANCE_V_REF_MAX_ZD
|
||||
#define GUIDANCE_V_REF_MAX_ZD ( 3.)
|
||||
#endif
|
||||
#define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
|
||||
|
||||
/* second order model natural frequency and damping */
|
||||
#ifndef GUIDANCE_V_REF_OMEGA
|
||||
#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
|
||||
#endif
|
||||
#ifndef GUIDANCE_V_REF_ZETA
|
||||
#define GUIDANCE_V_REF_ZETA 0.85
|
||||
#endif
|
||||
#define GV_ZETA_OMEGA_FRAC 10
|
||||
#define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
|
||||
#define GV_OMEGA_2_FRAC 7
|
||||
#define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
|
||||
|
||||
/* first order time constant */
|
||||
#define GV_REF_THAU_F 0.25
|
||||
#define GV_REF_INV_THAU_FRAC 16
|
||||
#define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)
|
||||
|
||||
#ifdef GUIDANCE_V_C
|
||||
static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
|
||||
static inline void gv_update_ref_from_z_sp(int32_t z_sp);
|
||||
static inline void gv_update_ref_from_zd_sp(int32_t zd_sp);
|
||||
|
||||
int64_t gv_z_ref;
|
||||
int32_t gv_zd_ref;
|
||||
int32_t gv_zdd_ref;
|
||||
|
||||
__attribute__ ((always_inline)) static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
|
||||
int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC);
|
||||
gv_z_ref = new_z;
|
||||
gv_zd_ref = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
|
||||
gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
|
||||
}
|
||||
|
||||
__attribute__ ((always_inline)) static inline void gv_update_ref_from_z_sp(int32_t z_sp) {
|
||||
|
||||
gv_z_ref += gv_zd_ref;
|
||||
gv_zd_ref += gv_zdd_ref;
|
||||
|
||||
// compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
|
||||
int32_t zd_zdd_res = gv_zd_ref>>(GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC);
|
||||
int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC);
|
||||
// compute z error in z_sp resolution
|
||||
int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC));
|
||||
// convert to accel resolution
|
||||
int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC);
|
||||
int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC;
|
||||
gv_zdd_ref = zdd_speed + zdd_pos;
|
||||
|
||||
/* Saturate accelerations */
|
||||
Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gv_zd_ref <= GV_MIN_ZD) {
|
||||
gv_zd_ref = GV_MIN_ZD;
|
||||
if (gv_zdd_ref < 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
else if (gv_zd_ref >= GV_MAX_ZD) {
|
||||
gv_zd_ref = GV_MAX_ZD;
|
||||
if (gv_zdd_ref > 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
__attribute__ ((always_inline)) static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) {
|
||||
|
||||
gv_z_ref += gv_zd_ref;
|
||||
gv_zd_ref += gv_zdd_ref;
|
||||
|
||||
int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
|
||||
int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC);
|
||||
gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC;
|
||||
|
||||
/* Saturate accelerations */
|
||||
Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
|
||||
|
||||
/* Saturate speed and adjust acceleration accordingly */
|
||||
if (gv_zd_ref <= GV_MIN_ZD) {
|
||||
gv_zd_ref = GV_MIN_ZD;
|
||||
if (gv_zdd_ref < 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
else if (gv_zd_ref >= GV_MAX_ZD) {
|
||||
gv_zd_ref = GV_MAX_ZD;
|
||||
if (gv_zdd_ref > 0)
|
||||
gv_zdd_ref = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* GUIDANCE_V_C */
|
||||
extern void gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
|
||||
extern void gv_update_ref_from_z_sp(int32_t z_sp);
|
||||
extern void gv_update_ref_from_zd_sp(int32_t zd_sp);
|
||||
|
||||
#endif /* GUIDANCE_V_REF_H */
|
||||
|
||||
Reference in New Issue
Block a user