diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
index 61c924f2e6..7dbce7b3e6 100644
--- a/conf/airframes/tudelft/bebop_indi_actuators.xml
+++ b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -159,6 +159,7 @@
diff --git a/conf/modules/guidance_indi.xml b/conf/modules/guidance_indi.xml
index 26fc8cef8c..2b64816da2 100644
--- a/conf/modules/guidance_indi.xml
+++ b/conf/modules/guidance_indi.xml
@@ -9,8 +9,8 @@
-
-
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index b5c9bd6e4b..d427e70098 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -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.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);
- 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);
}
@@ -426,7 +432,10 @@ static void guidance_h_update_reference(void)
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF
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 {
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) {
/* 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_LSHIFT(guidance_h.ref.speed, gh_ref.speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
- INT32_VECT2_LSHIFT(guidance_h.ref.accel, gh_ref.accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
+ guidance_h.ref.speed.x = SPEED_BFP_OF_REAL(gh_ref.speed.x);
+ 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 {
VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
INT_VECT2_ZERO(guidance_h.ref.speed);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
index 26c2c63b7e..8f37ff7713 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
@@ -29,9 +29,7 @@
struct GuidanceHRef gh_ref;
-static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC);
-
-#define GH_MAX_SPEED_REF_FRAC 7
+static const float gh_max_accel = GUIDANCE_H_REF_MAX_ACCEL;
/** default second order model natural frequency */
#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
/** default second order model damping */
#ifndef GUIDANCE_H_REF_ZETA
-#define GUIDANCE_H_REF_ZETA 0.85
+#define GUIDANCE_H_REF_ZETA 0.85f
#endif
-#define GH_ZETA_OMEGA_FRAC 10
-#define GH_OMEGA_2_FRAC 7
-
-
/** first order time constant */
#ifndef GUIDANCE_H_REF_TAU
-#define GUIDANCE_H_REF_TAU 0.5
+#define GUIDANCE_H_REF_TAU 0.5f
#endif
-#define GH_REF_INV_TAU_FRAC 16
-static void gh_compute_route_ref(struct Int32Vect2 *ref_vector);
-static void gh_compute_ref_max(struct Int32Vect2 *ref_vector);
-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);
+static void gh_saturate_speed(struct FloatVect2 *speed_sp);
+static void gh_saturate_accel(struct FloatVect2 *accel_sp);
void gh_ref_init(void)
{
gh_ref.omega = GUIDANCE_H_REF_OMEGA;
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.omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC);
+ gh_ref.zeta_omega = GUIDANCE_H_REF_ZETA * GUIDANCE_H_REF_OMEGA;
+ gh_ref.omega_2 = GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA;
gh_set_tau(GUIDANCE_H_REF_TAU);
gh_set_max_speed(GUIDANCE_H_REF_MAX_SPEED);
+ gh_ref.dt = (1.0f/PERIODIC_FREQUENCY);
}
float gh_set_max_speed(float max_speed)
{
/* 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_int = BFP_OF_REAL(gh_ref.max_speed, GH_MAX_SPEED_REF_FRAC);
+ gh_ref.max_speed = Min(fabsf(max_speed), 100.0f);
return gh_ref.max_speed;
}
@@ -82,7 +71,7 @@ float gh_set_tau(float tau)
{
gh_ref.tau = tau;
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;
}
@@ -90,8 +79,8 @@ float gh_set_omega(float omega)
{
gh_ref.omega = omega;
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.zeta_omega = BFP_OF_REAL((gh_ref.zeta * gh_ref.omega), GH_ZETA_OMEGA_FRAC);
+ gh_ref.omega_2 = gh_ref.omega * gh_ref.omega;
+ gh_ref.zeta_omega = gh_ref.zeta * gh_ref.omega;
return gh_ref.omega;
}
@@ -99,172 +88,108 @@ float gh_set_zeta(float zeta)
{
gh_ref.zeta = zeta;
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;
}
-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;
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_ref.pos = new_pos;
- INT32_VECT2_RSHIFT(gh_ref.speed, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
- INT32_VECT2_RSHIFT(gh_ref.accel, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
+ VECT2_COPY(gh_ref.speed, speed);
+ VECT2_COPY(gh_ref.accel, accel);
}
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_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 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)
- struct Int32Vect2 speed;
- INT32_VECT2_RSHIFT(speed, gh_ref.speed, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
- VECT2_SMUL(speed, speed, -2 * gh_ref.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_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);
+ struct FloatVect2 accel_sp;
+ struct FloatVect2 speed_err;
+ VECT2_DIFF(speed_err, vel_sp, gh_ref.speed);
+ VECT2_SMUL(accel_sp, speed_err, 2 * gh_ref.zeta_omega);
- /* Compute max ref accel/speed along route before saturation */
- gh_compute_ref_max(&pos_err);
+ gh_saturate_accel(&accel_sp);
- gh_saturate_ref_accel();
- gh_saturate_ref_speed();
+ // copy accel
+ VECT2_COPY(gh_ref.accel, accel_sp);
}
-
-void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
+void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
{
- /* WARNING: SPEED SATURATION UNTESTED */
- VECT2_ADD(gh_ref.pos, gh_ref.speed);
- VECT2_ADD(gh_ref.speed, gh_ref.accel);
+ struct FloatVect2 pos_step, speed_step;
+
+ 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
- struct Int32Vect2 speed_err;
- INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
- 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));
+ struct FloatVect2 speed_err;
+ VECT2_DIFF(speed_err, gh_ref.speed, speed_sp);
// compute accel from speed_sp
- VECT2_SMUL(gh_ref.accel, speed_err, -gh_ref.inv_tau);
- INT32_VECT2_RSHIFT(gh_ref.accel, gh_ref.accel, GH_REF_INV_TAU_FRAC);
+ struct FloatVect2 accel_sp;
+ VECT2_SMUL(accel_sp, speed_err, -gh_ref.inv_tau);
- /* Compute max ref accel/speed along route before saturation */
- gh_compute_ref_max_speed(&speed_sp);
- gh_compute_ref_max_accel(&speed_err);
+ gh_saturate_accel(&accel_sp);
- gh_saturate_ref_accel();
- gh_saturate_ref_speed();
+ // copy accel
+ 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);
- gh_ref.route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
- /* 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);
-}
+ // Speed squared
+ float v_norm2 = VECT2_NORM2(*speed_sp);
-static void gh_compute_ref_max(struct Int32Vect2 *ref_vector)
-{
- /* Bound ref to max speed/accel along route reference angle.
- * 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;
- 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);
+ // Apply saturation if above max speed
+ if (v_norm2 > (gh_ref.max_speed * gh_ref.max_speed)) {
+ // speed_sp/sqrt(v_norm2)*vmax
+ float factor = gh_ref.max_speed / sqrtf(v_norm2);
+ VECT2_SMUL(*speed_sp, *speed_sp, factor);
}
}
-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.
- * 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_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));
-}
+ // Accel squared
+ float a_norm2 = VECT2_NORM2(*accel_sp);
-/** saturate reference accelerations */
-static void gh_saturate_ref_accel(void)
-{
- /* Saturate accelerations */
- BoundAbs(gh_ref.accel.x, gh_ref.max_accel.x);
- 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;
- }
+ // Apply saturation if above max speed
+ if (a_norm2 > (gh_max_accel * gh_max_accel)) {
+ // accel_sp/sqrt(a_norm2)*amax
+ float factor = gh_max_accel / sqrtf(a_norm2);
+ VECT2_SMUL(*accel_sp, *accel_sp, factor);
}
}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
index 5ea3fd29be..0709173bfa 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
@@ -31,6 +31,7 @@
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
+#include "math/pprz_algebra_float.h"
/** Default speed saturation */
#ifndef GUIDANCE_H_REF_MAX_SPEED
@@ -49,29 +50,23 @@ extern float gh_max_speed;
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
#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_FREQ (1<