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

Ref model float (#2842)

Browse Source 
* reference model rewrite

* configurable carrot distance

* better CARROT_DIST int cast

* comment for POS_REF_FRAC

* higher resolution ref pos

Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
This commit is contained in:
Ewoud Smeur
2022-03-24 17:25:51 +01:00
committed by GitHub
parent 4d4fbc2852
commit 57ff4b9823
7 changed files with 127 additions and 195 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_indi_actuators.xml
+1
View File
@@ -159,6 +159,7 @@
<section name="NAVIGATION" prefix="NAV_"> <section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="2.5"/> <define name="CLIMB_VSPEED" value="2.5"/>
<define name="DESCEND_VSPEED" value="-1.0"/> <define name="DESCEND_VSPEED" value="-1.0"/>
<define name="CARROT_DIST" value="100"/>
</section> </section>
<section name="SIMULATOR" prefix="NPS_"> <section name="SIMULATOR" prefix="NPS_">
conf/modules/guidance_indi.xml
+2 -2
View File
@@ -9,8 +9,8 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="guidance_indi"> <dl_settings NAME="guidance_indi">
<dl_setting var="guidance_indi_pos_gain" min="0" step="0.1" max="10.0" shortname="kp_p" param="GUIDANCE_INDI_POS_GAIN" persistent="true"/> <dl_setting var="guidance_indi_pos_gain" min="0" step="0.1" max="10.0" shortname="kp" param="GUIDANCE_INDI_POS_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_speed_gain" min="0" step="0.1" max="10.0" shortname="kp_p" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/> <dl_setting var="guidance_indi_speed_gain" min="0" step="0.1" max="10.0" shortname="kd" param="GUIDANCE_INDI_SPEED_GAIN" persistent="true"/>
<dl_setting var="guidance_indi_max_bank" min="0" step="0.1" max="80.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK"/> <dl_setting var="guidance_indi_max_bank" min="0" step="0.1" max="80.0" shortname="max_bank" param="GUIDANCE_H_MAX_BANK"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+15 -4
View File
@@ -213,8 +213,14 @@ static inline void reset_guidance_reference_from_current_position(void)
{ {
VECT2_COPY(guidance_h.ref.pos, *stateGetPositionNed_i()); VECT2_COPY(guidance_h.ref.pos, *stateGetPositionNed_i());
VECT2_COPY(guidance_h.ref.speed, *stateGetSpeedNed_i()); VECT2_COPY(guidance_h.ref.speed, *stateGetSpeedNed_i());
struct FloatVect2 ref_speed;
ref_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.x);
ref_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.y);
INT_VECT2_ZERO(guidance_h.ref.accel); INT_VECT2_ZERO(guidance_h.ref.accel);
gh_set_ref(guidance_h.ref.pos, guidance_h.ref.speed, guidance_h.ref.accel); struct FloatVect2 ref_accel;
FLOAT_VECT2_ZERO(ref_accel);
gh_set_ref(guidance_h.ref.pos, ref_speed, ref_accel);
INT_VECT2_ZERO(guidance_h_trim_att_integrator); INT_VECT2_ZERO(guidance_h_trim_att_integrator);
} }
@@ -426,7 +432,10 @@ static void guidance_h_update_reference(void)
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */ /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF #if GUIDANCE_H_USE_REF
if (bit_is_set(guidance_h.sp.mask, 5)) { if (bit_is_set(guidance_h.sp.mask, 5)) {
gh_update_ref_from_speed_sp(guidance_h.sp.speed); struct FloatVect2 sp_speed;
sp_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.x);
sp_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.y);
gh_update_ref_from_speed_sp(sp_speed);
} else { } else {
gh_update_ref_from_pos_sp(guidance_h.sp.pos); gh_update_ref_from_pos_sp(guidance_h.sp.pos);
} }
@@ -436,8 +445,10 @@ static void guidance_h_update_reference(void)
if (guidance_h.use_ref) { if (guidance_h.use_ref) {
/* convert our reference to generic representation */ /* convert our reference to generic representation */
INT32_VECT2_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC)); INT32_VECT2_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h.ref.speed, gh_ref.speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); guidance_h.ref.speed.x = SPEED_BFP_OF_REAL(gh_ref.speed.x);
INT32_VECT2_LSHIFT(guidance_h.ref.accel, gh_ref.accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); guidance_h.ref.speed.y = SPEED_BFP_OF_REAL(gh_ref.speed.y);
guidance_h.ref.accel.x = ACCEL_BFP_OF_REAL(gh_ref.accel.x);
guidance_h.ref.accel.y = ACCEL_BFP_OF_REAL(gh_ref.accel.y);
} else { } else {
VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos); VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h.ref.speed); INT_VECT2_ZERO(guidance_h.ref.speed);
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
+80 -155
View File
@@ -29,9 +29,7 @@
struct GuidanceHRef gh_ref; struct GuidanceHRef gh_ref;
static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); static const float gh_max_accel = GUIDANCE_H_REF_MAX_ACCEL;
#define GH_MAX_SPEED_REF_FRAC 7
/** default second order model natural frequency */ /** default second order model natural frequency */
#ifndef GUIDANCE_H_REF_OMEGA #ifndef GUIDANCE_H_REF_OMEGA
@@ -39,42 +37,33 @@ static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACC
#endif #endif
/** default second order model damping */ /** default second order model damping */
#ifndef GUIDANCE_H_REF_ZETA #ifndef GUIDANCE_H_REF_ZETA
#define GUIDANCE_H_REF_ZETA 0.85 #define GUIDANCE_H_REF_ZETA 0.85f
#endif #endif
#define GH_ZETA_OMEGA_FRAC 10
#define GH_OMEGA_2_FRAC 7
/** first order time constant */ /** first order time constant */
#ifndef GUIDANCE_H_REF_TAU #ifndef GUIDANCE_H_REF_TAU
#define GUIDANCE_H_REF_TAU 0.5 #define GUIDANCE_H_REF_TAU 0.5f
#endif #endif
#define GH_REF_INV_TAU_FRAC 16
static void gh_compute_route_ref(struct Int32Vect2 *ref_vector); static void gh_saturate_speed(struct FloatVect2 *speed_sp);
static void gh_compute_ref_max(struct Int32Vect2 *ref_vector); static void gh_saturate_accel(struct FloatVect2 *accel_sp);
static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector);
static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector);
static void gh_saturate_ref_accel(void);
static void gh_saturate_ref_speed(void);
void gh_ref_init(void) void gh_ref_init(void)
{ {
gh_ref.omega = GUIDANCE_H_REF_OMEGA; gh_ref.omega = GUIDANCE_H_REF_OMEGA;
gh_ref.zeta = GUIDANCE_H_REF_ZETA; gh_ref.zeta = GUIDANCE_H_REF_ZETA;
gh_ref.zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA * GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC); gh_ref.zeta_omega = GUIDANCE_H_REF_ZETA * GUIDANCE_H_REF_OMEGA;
gh_ref.omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC); gh_ref.omega_2 = GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA;
gh_set_tau(GUIDANCE_H_REF_TAU); gh_set_tau(GUIDANCE_H_REF_TAU);
gh_set_max_speed(GUIDANCE_H_REF_MAX_SPEED); gh_set_max_speed(GUIDANCE_H_REF_MAX_SPEED);
gh_ref.dt = (1.0f/PERIODIC_FREQUENCY);
} }
float gh_set_max_speed(float max_speed) float gh_set_max_speed(float max_speed)
{ {
/* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */ /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */
gh_ref.max_speed = Min(fabs(max_speed), 100.0f); gh_ref.max_speed = Min(fabsf(max_speed), 100.0f);
gh_ref.max_speed_int = BFP_OF_REAL(gh_ref.max_speed, GH_MAX_SPEED_REF_FRAC);
return gh_ref.max_speed; return gh_ref.max_speed;
} }
@@ -82,7 +71,7 @@ float gh_set_tau(float tau)
{ {
gh_ref.tau = tau; gh_ref.tau = tau;
Bound(gh_ref.tau, 0.01f, 2.0f); Bound(gh_ref.tau, 0.01f, 2.0f);
gh_ref.inv_tau = BFP_OF_REAL((1. / gh_ref.tau), GH_REF_INV_TAU_FRAC); gh_ref.inv_tau = (1.f / gh_ref.tau);
return gh_ref.tau; return gh_ref.tau;
} }
@@ -90,8 +79,8 @@ float gh_set_omega(float omega)
{ {
gh_ref.omega = omega; gh_ref.omega = omega;
Bound(gh_ref.omega, 0.1f, 5.0f); Bound(gh_ref.omega, 0.1f, 5.0f);
gh_ref.omega_2 = BFP_OF_REAL((gh_ref.omega * gh_ref.omega), GH_OMEGA_2_FRAC); gh_ref.omega_2 = gh_ref.omega * gh_ref.omega;
gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta * gh_ref.omega), GH_ZETA_OMEGA_FRAC); gh_ref.zeta_omega = gh_ref.zeta * gh_ref.omega;
return gh_ref.omega; return gh_ref.omega;
} }
@@ -99,172 +88,108 @@ float gh_set_zeta(float zeta)
{ {
gh_ref.zeta = zeta; gh_ref.zeta = zeta;
Bound(gh_ref.zeta, 0.7f, 1.2f); Bound(gh_ref.zeta, 0.7f, 1.2f);
gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta * gh_ref.omega), GH_ZETA_OMEGA_FRAC); gh_ref.zeta_omega = gh_ref.zeta * gh_ref.omega;
return gh_ref.zeta; return gh_ref.zeta;
} }
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
{ {
struct Int64Vect2 new_pos; struct Int64Vect2 new_pos;
new_pos.x = ((int64_t)pos.x) << (GH_POS_REF_FRAC - INT32_POS_FRAC); 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); new_pos.y = ((int64_t)pos.y) << (GH_POS_REF_FRAC - INT32_POS_FRAC);
gh_ref.pos = new_pos; gh_ref.pos = new_pos;
INT32_VECT2_RSHIFT(gh_ref.speed, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); VECT2_COPY(gh_ref.speed, speed);
INT32_VECT2_RSHIFT(gh_ref.accel, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); VECT2_COPY(gh_ref.accel, accel);
} }
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
{ {
struct FloatVect2 pos_step, speed_step;
VECT2_ADD(gh_ref.pos, gh_ref.speed); VECT2_SMUL(pos_step, gh_ref.speed, gh_ref.dt);
VECT2_ADD(gh_ref.speed, gh_ref.accel); VECT2_SMUL(speed_step, gh_ref.accel, gh_ref.dt);
struct Int64Vect2 pos_update;
pos_update.x = BFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
pos_update.y = BFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);
VECT2_ADD(gh_ref.pos, pos_update);
VECT2_ADD(gh_ref.speed, speed_step);
// compute pos error in pos_frac resolution
struct FloatVect2 pos_err;
pos_err.x = POS_FLOAT_OF_BFP(pos_sp.x - (gh_ref.pos.x >> (GH_POS_REF_FRAC - INT32_POS_FRAC)));
pos_err.y = POS_FLOAT_OF_BFP(pos_sp.y - (gh_ref.pos.y >> (GH_POS_REF_FRAC - INT32_POS_FRAC)));
// Calculate velocity error
struct FloatVect2 vel_sp;
VECT2_SMUL(vel_sp, pos_err, gh_ref.omega*0.5/gh_ref.zeta);
// Saturate vel_sp
gh_saturate_speed(&vel_sp);
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed; struct FloatVect2 accel_sp;
INT32_VECT2_RSHIFT(speed, gh_ref.speed, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); struct FloatVect2 speed_err;
VECT2_SMUL(speed, speed, -2 * gh_ref.zeta_omega); VECT2_DIFF(speed_err, vel_sp, gh_ref.speed);
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); VECT2_SMUL(accel_sp, speed_err, 2 * gh_ref.zeta_omega);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
INT32_VECT2_RSHIFT(pos_err, gh_ref.pos, (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_ref.omega_2);
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(gh_ref.accel, speed, pos);
/* Compute max ref accel/speed along route before saturation */ gh_saturate_accel(&accel_sp);
gh_compute_ref_max(&pos_err);
gh_saturate_ref_accel(); // copy accel
gh_saturate_ref_speed(); VECT2_COPY(gh_ref.accel, accel_sp);
} }
void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
{ {
/* WARNING: SPEED SATURATION UNTESTED */ struct FloatVect2 pos_step, speed_step;
VECT2_ADD(gh_ref.pos, gh_ref.speed);
VECT2_ADD(gh_ref.speed, gh_ref.accel); VECT2_SMUL(pos_step, gh_ref.speed, gh_ref.dt);
VECT2_SMUL(speed_step, gh_ref.accel, gh_ref.dt);
struct Int64Vect2 pos_update;
pos_update.x = BFP_OF_REAL(pos_step.x, GH_POS_REF_FRAC);
pos_update.y = BFP_OF_REAL(pos_step.y, GH_POS_REF_FRAC);
VECT2_ADD(gh_ref.pos, pos_update);
VECT2_ADD(gh_ref.speed, speed_step);
// compute speed error // compute speed error
struct Int32Vect2 speed_err; struct FloatVect2 speed_err;
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); VECT2_DIFF(speed_err, gh_ref.speed, speed_sp);
VECT2_DIFF(speed_err, gh_ref.speed, 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 // compute accel from speed_sp
VECT2_SMUL(gh_ref.accel, speed_err, -gh_ref.inv_tau); struct FloatVect2 accel_sp;
INT32_VECT2_RSHIFT(gh_ref.accel, gh_ref.accel, GH_REF_INV_TAU_FRAC); VECT2_SMUL(accel_sp, speed_err, -gh_ref.inv_tau);
/* Compute max ref accel/speed along route before saturation */ gh_saturate_accel(&accel_sp);
gh_compute_ref_max_speed(&speed_sp);
gh_compute_ref_max_accel(&speed_err);
gh_saturate_ref_accel(); // copy accel
gh_saturate_ref_speed(); VECT2_COPY(gh_ref.accel, accel_sp);
} }
static void gh_compute_route_ref(struct Int32Vect2 *ref_vector) static void gh_saturate_speed(struct FloatVect2 *speed_sp)
{ {
float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x); // Speed squared
gh_ref.route_ref = ANGLE_BFP_OF_REAL(f_route_ref); float v_norm2 = VECT2_NORM2(*speed_sp);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(gh_ref.s_route_ref, gh_ref.route_ref);
PPRZ_ITRIG_COS(gh_ref.c_route_ref, gh_ref.route_ref);
gh_ref.c_route_ref = abs(gh_ref.c_route_ref);
gh_ref.s_route_ref = abs(gh_ref.s_route_ref);
}
static void gh_compute_ref_max(struct Int32Vect2 *ref_vector) // Apply saturation if above max speed
{ if (v_norm2 > (gh_ref.max_speed * gh_ref.max_speed)) {
/* Bound ref to max speed/accel along route reference angle. // speed_sp/sqrt(v_norm2)*vmax
* If angle can't be computed, simply set both axes to max magnitude/sqrt(2). float factor = gh_ref.max_speed / sqrtf(v_norm2);
*/ VECT2_SMUL(*speed_sp, *speed_sp, factor);
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_ref.max_accel.x = gh_ref.max_accel.y = gh_max_accel * 0.707;
gh_ref.max_vel.x = gh_ref.max_vel.y = gh_ref.max_speed_int * 0.707;
} else {
gh_compute_route_ref(ref_vector);
/* Compute maximum acceleration*/
gh_ref.max_accel.x = INT_MULT_RSHIFT(gh_max_accel, gh_ref.c_route_ref, INT32_TRIG_FRAC);
gh_ref.max_accel.y = INT_MULT_RSHIFT(gh_max_accel, gh_ref.s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum reference x/y velocity from absolute max_speed */
gh_ref.max_vel.x = INT_MULT_RSHIFT(gh_ref.max_speed_int, gh_ref.c_route_ref, INT32_TRIG_FRAC);
gh_ref.max_vel.y = INT_MULT_RSHIFT(gh_ref.max_speed_int, gh_ref.s_route_ref, INT32_TRIG_FRAC);
}
/* restore gh_ref.speed range (Q14.17) */
INT32_VECT2_LSHIFT(gh_ref.max_vel, gh_ref.max_vel, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
}
static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector)
{
/* Bound ref to max accel along reference vector.
* If angle can't be computed, simply set both axes to max magnitude/sqrt(2).
*/
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_ref.max_accel.x = gh_ref.max_accel.y = gh_max_accel * 0.707;
} else {
gh_compute_route_ref(ref_vector);
/* Compute maximum acceleration*/
gh_ref.max_accel.x = INT_MULT_RSHIFT(gh_max_accel, gh_ref.c_route_ref, INT32_TRIG_FRAC);
gh_ref.max_accel.y = INT_MULT_RSHIFT(gh_max_accel, gh_ref.s_route_ref, INT32_TRIG_FRAC);
} }
} }
static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector) static void gh_saturate_accel(struct FloatVect2 *accel_sp)
{ {
/* Bound ref to max speed along reference vector. // Accel squared
* If angle can't be computed, simply set both axes to max magnitude/sqrt(2). float a_norm2 = VECT2_NORM2(*accel_sp);
*/
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_ref.max_vel.x = gh_ref.max_vel.y = gh_ref.max_speed_int * 0.707;
} else {
gh_compute_route_ref(ref_vector);
/* Compute maximum reference x/y velocity from absolute max_speed */
gh_ref.max_vel.x = INT_MULT_RSHIFT(gh_ref.max_speed_int, gh_ref.c_route_ref, INT32_TRIG_FRAC);
gh_ref.max_vel.y = INT_MULT_RSHIFT(gh_ref.max_speed_int, gh_ref.s_route_ref, INT32_TRIG_FRAC);
}
/* restore gh_ref.speed range (Q14.17) */
INT32_VECT2_LSHIFT(gh_ref.max_vel, gh_ref.max_vel, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
}
/** saturate reference accelerations */ // Apply saturation if above max speed
static void gh_saturate_ref_accel(void) if (a_norm2 > (gh_max_accel * gh_max_accel)) {
{ // accel_sp/sqrt(a_norm2)*amax
/* Saturate accelerations */ float factor = gh_max_accel / sqrtf(a_norm2);
BoundAbs(gh_ref.accel.x, gh_ref.max_accel.x); VECT2_SMUL(*accel_sp, *accel_sp, factor);
BoundAbs(gh_ref.accel.y, gh_ref.max_accel.y);
}
/** Saturate ref speed and adjust acceleration accordingly */
static void gh_saturate_ref_speed(void)
{
if (gh_ref.speed.x < -gh_ref.max_vel.x) {
gh_ref.speed.x = -gh_ref.max_vel.x;
if (gh_ref.accel.x < 0) {
gh_ref.accel.x = 0;
}
} else if (gh_ref.speed.x > gh_ref.max_vel.x) {
gh_ref.speed.x = gh_ref.max_vel.x;
if (gh_ref.accel.x > 0) {
gh_ref.accel.x = 0;
}
}
if (gh_ref.speed.y < -gh_ref.max_vel.y) {
gh_ref.speed.y = -gh_ref.max_vel.y;
if (gh_ref.accel.y < 0) {
gh_ref.accel.y = 0;
}
} else if (gh_ref.speed.y > gh_ref.max_vel.y) {
gh_ref.speed.y = gh_ref.max_vel.y;
if (gh_ref.accel.y > 0) {
gh_ref.accel.y = 0;
}
} }
} }
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+14 -27
View File
@@ -31,6 +31,7 @@
#include "math/pprz_algebra.h" #include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "generated/airframe.h" #include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
/** Default speed saturation */ /** Default speed saturation */
#ifndef GUIDANCE_H_REF_MAX_SPEED #ifndef GUIDANCE_H_REF_MAX_SPEED
@@ -49,29 +50,23 @@ extern float gh_max_speed;
#define GUIDANCE_H_REF_MAX_ACCEL 5.66 #define GUIDANCE_H_REF_MAX_ACCEL 5.66
#endif #endif
/** Update frequency /** fixedpoint representation: Q26.37 will give a range of
* 67e3 km and a resolution of 1.5e-11 m. At a rate of 500Hz,
* a ref speed of 7.3e-9 m/s could still update the position.
*/ */
#define GH_FREQ_FRAC 9 #define GH_POS_REF_FRAC 37
#define GH_FREQ (1<<GH_FREQ_FRAC)
#define GH_ACCEL_REF_FRAC 8
#define GH_SPEED_REF_FRAC (GH_ACCEL_REF_FRAC + GH_FREQ_FRAC)
#define GH_POS_REF_FRAC (GH_SPEED_REF_FRAC + GH_FREQ_FRAC)
struct GuidanceHRef { struct GuidanceHRef {
/** Reference model acceleration. /** Reference model acceleration.
* in meters/sec2 (output) * in meters/sec2 (output)
* fixed point representation: Q23.8
* accuracy 0.0039, range 8388km/s2
*/ */
struct Int32Vect2 accel; struct FloatVect2 accel;
/** Reference model speed. /** Reference model speed.
* in meters/sec * in meters/sec
* with fixedpoint representation: Q14.17
* accuracy 0.0000076 , range 16384m/s * accuracy 0.0000076 , range 16384m/s
*/ */
struct Int32Vect2 speed; struct FloatVect2 speed;
/** Reference model position. /** Reference model position.
* in meters * in meters
@@ -91,29 +86,21 @@ struct GuidanceHRef {
/* /*
* internal variables * internal variables
*/ */
int32_t zeta_omega; float zeta_omega;
int32_t omega_2; float omega_2;
int32_t inv_tau; float inv_tau;
struct Int32Vect2 max_vel; /** Integration timestep
struct Int32Vect2 max_accel;
/** gh_max_speed in fixed point representation with #GH_MAX_SPEED_REF_FRAC
* must be limited to 2^14 to avoid overflow
*/ */
int32_t max_speed_int; float dt;
int32_t route_ref;
int32_t s_route_ref;
int32_t c_route_ref;
}; };
extern struct GuidanceHRef gh_ref; extern struct GuidanceHRef gh_ref;
extern void gh_ref_init(void); extern void gh_ref_init(void);
extern void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel); extern void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel);
extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp); extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
extern void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp); extern void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp);
/** /**
* Set a new maximum speed for waypoint navigation. * Set a new maximum speed for waypoint navigation.
sw/airborne/firmwares/rotorcraft/navigation.c
+5 -1
View File
@@ -74,8 +74,12 @@ PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
#define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME) #define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
#endif #endif
#ifndef NAV_CARROT_DIST
#define NAV_CARROT_DIST 12
#endif
#define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC) #define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
#define CARROT_DIST (12 << INT32_POS_FRAC) #define CARROT_DIST ((int32_t) POS_BFP_OF_REAL(NAV_CARROT_DIST))
bool force_forward = false; bool force_forward = false;
sw/airborne/modules/gps/gps_sim_hitl.c
+10 -6
View File
@@ -49,9 +49,11 @@ void gps_sim_hitl_event(void)
gps.last_msg_time = sys_time.nb_sec; gps.last_msg_time = sys_time.nb_sec;
if (state.ned_initialized_i) { if (state.ned_initialized_i) {
if (!autopilot_in_flight()) { if (!autopilot_in_flight()) {
struct Int32Vect2 zero_vector; struct Int32Vect2 zero_vector_i;
INT_VECT2_ZERO(zero_vector); struct FloatVect2 zero_vector_f;
gh_set_ref(zero_vector, zero_vector, zero_vector); INT_VECT2_ZERO(zero_vector_i);
FLOAT_VECT2_ZERO(zero_vector_f);
gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f);
INT_VECT2_ZERO(guidance_h.ref.pos); INT_VECT2_ZERO(guidance_h.ref.pos);
INT_VECT2_ZERO(guidance_h.ref.speed); INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel); INT_VECT2_ZERO(guidance_h.ref.accel);
@@ -77,9 +79,11 @@ void gps_sim_hitl_event(void)
gps_available = true; gps_available = true;
} }
else { else {
struct Int32Vect2 zero_vector; struct Int32Vect2 zero_vector_i;
INT_VECT2_ZERO(zero_vector); struct FloatVect2 zero_vector_f;
gh_set_ref(zero_vector, zero_vector, zero_vector); INT_VECT2_ZERO(zero_vector_i);
FLOAT_VECT2_ZERO(zero_vector_f);
gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f);
gv_set_ref(0, 0, 0); gv_set_ref(0, 0, 0);
} }