diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 2471d0f3a9..11cf80a2bb 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -71,7 +71,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); \ + guidance_h_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); \ @@ -194,7 +194,7 @@ void guidance_h_run(bool_t in_flight) { else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); #ifdef GUIDANCE_H_USE_REF - b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp); + guidance_h_update_ref_from_pos_sp(guidance_h_pos_sp); #endif #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); @@ -283,9 +283,9 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in /* convert our reference to generic representation */ #ifdef GUIDANCE_H_USE_REF - 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)); + INT32_VECT2_RSHIFT(guidance_h_pos_ref, guidance_h_pos_ref, (GUIDANCE_H_POS_REF_FRAC - INT32_POS_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_speed_ref, guidance_h_speed_ref, (INT32_SPEED_FRAC - GUIDANCE_H_SPEED_REF_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_accel_ref, guidance_h_accel_ref, (INT32_ACCEL_FRAC - GUIDANCE_H_ACCEL_REF_FRAC)); #else VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp); INT_VECT2_ZERO(guidance_h_speed_ref); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index 1536d9c76b..53bf2eae83 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -30,35 +30,35 @@ #include "math/pprz_algebra_int.h" /* update frequency */ -#define B2_GH_FREQ_FRAC 9 -#define B2_GH_FREQ (1<= 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 (guidance_h_speed_ref.x >= GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.x = GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.x > 0) + guidance_h_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; + if (guidance_h_speed_ref.y <= -GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.y = -GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.y < 0) + guidance_h_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; + else if (guidance_h_speed_ref.y >= GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.y = GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.y > 0) + guidance_h_accel_ref.y = 0; } } -static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { +static inline void guidance_h_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); + VECT2_ADD(guidance_h_pos_ref, guidance_h_speed_ref); + VECT2_ADD(guidance_h_speed_ref, guidance_h_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); + INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GUIDANCE_H_SPEED_REF_FRAC)); + VECT2_DIFF(speed_err, guidance_h_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)); + INT32_VECT2_RSHIFT(speed_err, speed_err, (GUIDANCE_H_SPEED_REF_FRAC - GUIDANCE_H_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); + VECT2_SMUL(guidance_h_accel_ref, speed_err, -GUIDANCE_H_REF_INV_THAU); + INT32_VECT2_RSHIFT(guidance_h_accel_ref, guidance_h_accel_ref, GUIDANCE_H_REF_INV_THAU_FRAC); /* Saturate accelerations */ - VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL); + VECT2_STRIM(guidance_h_accel_ref, -GUIDANCE_H_MAX_ACCEL, GUIDANCE_H_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; + if (guidance_h_speed_ref.x <= -GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.x = -GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.x < 0) + guidance_h_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; + else if (guidance_h_speed_ref.x >= GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.x = GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.x > 0) + guidance_h_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; + if (guidance_h_speed_ref.y <= -GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.y = -GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.y < 0) + guidance_h_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; + else if (guidance_h_speed_ref.y >= GUIDANCE_H_MAX_SPEED) { + guidance_h_speed_ref.y = GUIDANCE_H_MAX_SPEED; + if (guidance_h_accel_ref.y > 0) + guidance_h_accel_ref.y = 0; } }